Merge branch 'pince'

src/main/java/frc/robot/RobotContainer.java

@@ -30,6 +30,11 @@ import frc.robot.commands.AlgueExpire;
 import frc.robot.commands.Algue_inspire;
 import frc.robot.commands.AprilTag3;
 import frc.robot.commands.AprilTag3G;
+import frc.robot.commands.CorailAspir;
+import frc.robot.commands.CorailTest;
+import frc.robot.commands.CoralAlgueInspire;
+import frc.robot.commands.CoralExpire;
+import frc.robot.commands.Depart;
 import frc.robot.commands.ElevateurManuel;
 import frc.robot.commands.Forme3;
 import frc.robot.commands.L2;
@@ -38,6 +43,7 @@ import frc.robot.commands.L4;
 import frc.robot.commands.PinceManuel;
 import frc.robot.commands.PinceManuel2;
 import frc.robot.commands.RainBow;
+import frc.robot.commands.StationPince;
 import frc.robot.commands.reset;
 import frc.robot.subsystems.Bougie;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -69,7 +75,7 @@ public class RobotContainer {
   Elevateur elevateur = new Elevateur();
   Pince pince = new Pince();
   ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
-  PinceManuel pinceManuel = new PinceManuel(pince);
+  PinceManuel pinceManuel = new PinceManuel(pince,manette2::getRightY);
   PinceManuel2 pinceManuel2 = new PinceManuel2(pince);
   Bougie bougie = new Bougie();
   Limelight3G limelight3g = new Limelight3G();
@@ -83,10 +89,23 @@ public class RobotContainer {
     SmartDashboard.putData("Auto Mode", autoChooser);
     configureBindings();
     NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
+    NamedCommands.registerCommand("Station",new StationPince(pince, elevateur));
+    NamedCommands.registerCommand("L4", new L4(elevateur, pince));
+    NamedCommands.registerCommand("L3", new L3(elevateur, pince));
+    NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie));
+    NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie));
     }
   private void configureBindings() {
     // Elevateur manuel
-
+ drivetrain.setDefaultCommand(
+          // Drivetrain will execute this command periodically
+          drivetrain.applyRequest(() ->
+            drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward)
+              .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left)
+              .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left)
+        )
+      );
+    drivetrain.registerTelemetry(logger::telemeterize);
     elevateur.setDefaultCommand(new RunCommand(()->{
       elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2));
     }, elevateur));
@@ -96,23 +115,10 @@ public class RobotContainer {
       pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2));
     }, pince));
      
-      // Note that X is defined as forward according to WPILib convention,
-      // and Y is defined as to the left according to WPILib convention.
-      drivetrain.setDefaultCommand(
-          // Drivetrain will execute this command periodically
-          drivetrain.applyRequest(() ->
-            drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward)
-              .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left)
-              .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left)
-        )
-      );
-    drivetrain.registerTelemetry(logger::telemeterize);
-
     // reset the field-centric heading on left bumper press
     manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
     manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
     manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
-    manette1.a().whileTrue(new reset(elevateur,pince));
    
 
     manette2.a().whileTrue(new Algue_inspire(pince));
@@ -120,9 +126,32 @@ public class RobotContainer {
     manette2.x().whileTrue(new L3(elevateur, pince));
     manette2.y().whileTrue(new L4(elevateur, pince));
     manette2.rightBumper().whileTrue(new AlgueExpire(pince, bougie));
+    // manette1
+    manette1.a().whileTrue(new Depart(elevateur, pince));
+    manette1.b().whileTrue(new L2(elevateur,pince));
+    manette1.x().whileTrue(new L3(elevateur, pince));
+    manette1.y().whileTrue(new L4(elevateur, pince));
+    manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
+    manette1.rightBumper().whileTrue(new StationPince(pince, elevateur));
+    //manette2
+    manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie));
+    manette2.a().whileTrue(new CorailAspir(pince));
+    manette2.b().whileTrue(new CoralExpire(pince, bougie));
+    manette2.povLeft().whileTrue(new Algue1Test(pince));
+    manette2.povRight().whileTrue(new Algue2Test(pince));
+    manette2.x().whileTrue(new CorailTest(pince));
+    manette2.start().whileTrue(new reset(elevateur));
+  }
+  
+
+
+
+      // Note that X is defined as forward according to WPILib convention,
+      // and Y is defined as to the left according to WPILib convention.
+  
     
     // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
-  }  
+    
     public Command getAutonomousCommand() {
       return new SequentialCommandGroup(Commands.runOnce(()->{
         boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;

src/main/java/frc/robot/commands/PinceManuel.java

@@ -3,17 +3,19 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.commands;
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Pince;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class PinceManuel extends Command {
   private Pince pince;
+  private DoubleSupplier x;
   /** Creates a new PinceManuel. */
-  public PinceManuel(Pince pince
+  public PinceManuel(Pince pince, DoubleSupplier x) {
-  //, DoubleSupplier doubleSupplier
-  ) {
     this.pince = pince;
+    this.x = x;
     //this.doubleSupplier = doubleSupplier;
     addRequirements(pince);
     // Use addRequirements() here to declare subsystem dependencies.
@@ -26,12 +28,12 @@ public class PinceManuel extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    // if(pince.position()){
+     if(pince.position()){
-    //   pince.pivote(0);
+       pince.pivote(0);
-    // }
+     }
-    // else{
+     else{
-      pince.pivote(0.2);
+      pince.pivote(x.getAsDouble());
-    // }
+     }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/reset.java

@@ -15,7 +15,7 @@ public class reset extends Command {
   /** Creates a new reset. */
   private Elevateur elevateur;
   private Pince pince;
-  public reset(Elevateur elevateur, Pince pince) {
+  public reset(Elevateur elevateur) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.elevateur = elevateur;
     this.pince = pince;