Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

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

@@ -8,9 +8,12 @@ import static edu.wpi.first.units.Units.*;
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -38,12 +41,15 @@ import frc.robot.subsystems.Grimpeur;
 import frc.robot.subsystems.Lanceur;
 import frc.robot.subsystems.Led;
 import frc.robot.subsystems.LimeLight3;
+import frc.robot.subsystems.Limelight3G;
 
 public class RobotContainer {
+  private final SendableChooser<Command> autoChooser;
   Balayeuse balayeuse = new Balayeuse();
   Grimpeur grimpeur = new Grimpeur();
   Lanceur lanceur = new Lanceur();
   LimeLight3 limeLight3 = new LimeLight3();
+  Limelight3G limeLight3G = new Limelight3G();
   Led led = new Led();
   CommandXboxController manette = new CommandXboxController(0);
   private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -58,30 +64,37 @@ public class RobotContainer {
 
     private final Telemetry logger = new Telemetry(MaxSpeed);
 
-    private final CommandXboxController joystick = new CommandXboxController(0);
-
     public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
 
 
   public RobotContainer() {
+    autoChooser = AutoBuilder.buildAutoChooser();
     CameraServer.startAutomaticCapture();
     configureBindings();
-    NamedCommands.registerCommand("GrimperMur", new GrimperMur());
-    NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir());
+    NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
+    NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
     NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur));
     NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite());
     NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche());
-    NamedCommands.registerCommand("Limelighter", new Limelighter());
+    NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
     NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
     NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
-    NamedCommands.registerCommand("TournerAZero", new TournerAZero());
-    NamedCommands.registerCommand("TournerA180", new TournerA180());
+    NamedCommands.registerCommand("TournerAZero", new TournerAZero(drivetrain));
+    NamedCommands.registerCommand("TournerA180", new TournerA180(drivetrain));
     NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
     NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
   }
 
   private void configureBindings() {
-    manette.a().whileTrue(new Lancer(lanceur,limeLight3,balayeuse,led));
+    drivetrain.setDefaultCommand(
+          drivetrain.applyRequest(() ->
+            drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed, 0.05)) 
+              .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed, 0.05)) 
+             .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05))
+        )
+      );
+    manette.povUp().whileTrue(new LancerAuto(lanceur));
+    manette.a().whileTrue(new Lancer(lanceur,limeLight3G,balayeuse,led));
     manette.y().whileTrue(new ModeOposer(lanceur, balayeuse));
     manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3, balayeuse, led));
     manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur));
@@ -92,6 +105,6 @@ public class RobotContainer {
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return autoChooser.getSelected();
   }
 }

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

@@ -50,7 +50,7 @@ public class Aspirer extends Command {
     }
     if(moyenneAmp < balayeuse.AmpMax()){
       timer.reset();
-      balayeuse.Balayer(0.5);
+      balayeuse.Balayer(-0.5);
     }
     else{
         balayeuse.Balayer(0);

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

@@ -24,9 +24,12 @@ public class DescendreBalyeuse extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(balayeuse.Distance() < balayeuse.EncodeurBalayeuse()){
+    if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){
       balayeuse.Pivoter(-0.2);
     }
+    else{
+      balayeuse.Pivoter(0);
+    }
   }
 
   // Called once the command ends or is interrupted.

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

@@ -24,7 +24,7 @@ public class DescendreGrimpeur extends Command {
   @Override
   public void execute() {
     if(!grimpeur.Limit()){
-      grimpeur.Grimper(-0.5);
+      grimpeur.Grimper(-0.3);
     }
     else{
       grimpeur.Reset();

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

@@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.Lanceur;
 import frc.robot.subsystems.Led;
-import frc.robot.subsystems.LimeLight3;
 import frc.robot.subsystems.Limelight3G;
 
 /* 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 */
@@ -23,7 +22,7 @@ public class Lancer extends Command {
   private Led led;
   private double temp;
   /** Creates a new Lancer. */
-  public Lancer(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) {
+  public Lancer(Lanceur lanceur, Limelight3G limeLight3G, Balayeuse balayeuse,Led led) {
     this.lanceur = lanceur;
     this.balayeuse = balayeuse;
     this.led = led;
@@ -37,7 +36,7 @@ public class Lancer extends Command {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pidController = new PIDController(0, 0,0, 0);
+    pidController = new PIDController(1, 0,0, 0);
     timer.reset();
     timer.start();
     temp = lanceur.Amp();    
@@ -46,6 +45,14 @@ public class Lancer extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    double botx = 0;
+    double boty = 0;
+    double[] BotPose = new double[6];
+    if(limeLight3G.getV()){
+      BotPose = limeLight3G.getBotPoseBlue();
+      botx = BotPose[0];
+      boty = BotPose[1];
+    }
     int nbFois = 0;
 
     double moyenneAmp = 0;
@@ -59,19 +66,25 @@ public class Lancer extends Command {
       moyenneAmp += balayeuse.Amp() / nbFois;
       temp = balayeuse.Amp();
     }
-    if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
+    // if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
       timer.reset();
-      balayeuse.Balayer(0.5);
-      led.Jaune2();
-    }
-    else{
-      double vitesse = (100-limeLight3G.getTA())/lanceur.Vitesse();
-      double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+      balayeuse.Balayer(-0.5);
+      // led.Jaune2();
+    // }
+    // else{
+                                                
+         double vitesse = 0.5;
+         if(limeLight3G.getV()){
+                                          //pythagore |
+                                          //         \/
+            vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
+         }                                   
+      double output = vitesse; /*pidController.calculate(lanceur.Vitesse(),vitesse);*/
       lanceur.Lancer(output);
       if(lanceur.Vitesse() >= vitesse){
         lanceur.Demeler(0.5);
       }
-    }
+    // }
 
   }
 

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

@@ -37,7 +37,7 @@ public class LancerBaseVitesse extends Command {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pidController = new PIDController(0, 0,0, 0);
+    pidController = new PIDController(1, 0,0, 0);
     timer.reset();
     timer.start();
     temp = lanceur.Amp();    
@@ -46,32 +46,32 @@ public class LancerBaseVitesse extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    int nbFois = 0;
+    // int nbFois = 0;
 
-    double moyenneAmp = 0;
-    if(timer.get() < 3){
-      nbFois++;
-      moyenneAmp += balayeuse.Amp() / nbFois;
-    }
-    else{
-      nbFois++;
-      moyenneAmp -= temp;
-      moyenneAmp += balayeuse.Amp() / nbFois;
-      temp = balayeuse.Amp();
-    }
-    if(moyenneAmp > 30 && nbFois > 10){
-      timer.reset();
-      balayeuse.Balayer(0.5);
-      led.Jaune2();
-    }
-    else{
+    // double moyenneAmp = 0;
+    // if(timer.get() < 3){
+    //   nbFois++;
+    //   moyenneAmp += balayeuse.Amp() / nbFois;
+    // }
+    // else{
+    //   nbFois++;
+    //   moyenneAmp -= temp;
+    //   moyenneAmp += balayeuse.Amp() / nbFois;
+    //   temp = balayeuse.Amp();
+    // }
+    // if(moyenneAmp > 30 && nbFois > 10){
+    //   timer.reset();
+    //   balayeuse.Balayer(0.5);
+    //   led.Jaune2();
+    // }
+    // else{
       double vitesse = 0.4;
       double output = pidController.calculate(lanceur.Vitesse(),vitesse);
       lanceur.Lancer(output);
       if(lanceur.Vitesse() >= vitesse){
         lanceur.Demeler(0.5);
       }
-    }
+    // }
 
   }
 

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

@@ -4,12 +4,34 @@
 
 package frc.robot.commands;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Limelight3G;
+import static edu.wpi.first.units.Units.*;
 
 /* 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 Limelighter extends Command {
+  Timer timer;
+  Limelight3G limelight3g;
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+    double botx;
+    double boty;
+    double angle;
+    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
   /** Creates a new Limelighter. */
-  public Limelighter() {
+  public Limelighter(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain) {
+    this.limelight3g = limelight3g;
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain,limelight3g);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,15 +41,31 @@ public class Limelighter extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    double[] BotPose = new double[6];
+    BotPose = limelight3g.getBotPoseBlue();
+    botx = BotPose[0];
+    boty = BotPose[1];
+    angle = BotPose[5];
+    drivetrain.setControl(drive.withRotationalRate(limelight3g.Calcule(4.625594, botx, 4.034536, boty, BotPose[5])/90));
+    if(limelight3g.Calcule(4.625594, botx, 4.034536, boty, angle)/90 < 0.1){
+      timer.start();
+    }
+    else{
+      timer.reset();
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withRotationalRate(0));
+    timer.stop();
+  }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return timer.get() > 1;
   }
 }

src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java

@@ -4,12 +4,37 @@
 
 package frc.robot.commands.ModeAuto;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.LimeLight3;
 
 /* 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 GrimperMur extends Command {
+  LimeLight3 limeLight3;
+  CommandSwerveDrivetrain drivetrain;
+  double[] BotPose = new double[6];
+  double botx;
+  double boty;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
   /** Creates a new GrimperMur. */
-  public GrimperMur() {
+  public GrimperMur(LimeLight3 limeLight3, CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    this.limeLight3 = limeLight3;
+    addRequirements(limeLight3,drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,15 +44,27 @@ public class GrimperMur extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    BotPose = limeLight3.getBotPoseBlue();
+    botx = BotPose[0];
+    boty = BotPose[1];
+    if(180-pigeon2.getYaw().getValueAsDouble() < 10 && 180- pigeon2.getYaw().getValueAsDouble() > -10){
+      drivetrain.setControl(drive.withVelocityX(2.961328-boty).withVelocityY(1.11-botx));
+    }
+    else{
+       drivetrain.setControl(drive.withRotationalRate(180-pigeon2.getYaw().getValueAsDouble()/45));
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
+  }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return (2.961328-boty < 0.05 || 2.961328-boty >-0.05) && (1.11-botx < 0.05 || 1.11-botx > -0.05);
   }
 }

src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java

@@ -4,12 +4,37 @@
 
 package frc.robot.commands.ModeAuto;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Limelight3G;
 
 /* 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 GrimperReservoir extends Command {
-  /** Creates a new GrimperReservoir. */
-  public GrimperReservoir() {
+  Limelight3G limeLight3G;
+  CommandSwerveDrivetrain drivetrain;
+  double[] BotPose = new double[6];
+  double botx;
+  double boty;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new GrimperMur. */
+  public GrimperReservoir(Limelight3G limeLight3G, CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    this.limeLight3G = limeLight3G;
+    addRequirements(limeLight3G,drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,15 +44,27 @@ public class GrimperReservoir extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    BotPose = limeLight3G.getBotPoseBlue();
+    botx = BotPose[0];
+    boty = BotPose[1];
+    if(0-pigeon2.getYaw().getValueAsDouble() < 10 && 0- pigeon2.getYaw().getValueAsDouble() > -10){
+      drivetrain.setControl(drive.withVelocityX(5.081328-boty).withVelocityY(1.11-botx));
+    }
+    else{
+       drivetrain.setControl(drive.withRotationalRate(0-pigeon2.getYaw().getValueAsDouble()/45));
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
+  }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return 5.081328-boty < 0.05 || 5.081328-boty >-0.05 && 1.11-botx < 0.05 || 1.11-botx > -0.05;
   }
 }

src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java

@@ -15,6 +15,7 @@ public class LancerAuto extends Command {
   /** Creates a new LancerAuto. */
   public LancerAuto(Lanceur lanceur) {
     this.lanceur = lanceur;
+    addRequirements(lanceur);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -28,12 +29,16 @@ public class LancerAuto extends Command {
   @Override
   public void execute() {
     lanceur.Lancer(0.5);
+    if(timer.get() > 1){
+      lanceur.Demeler(0.5);
+    }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     lanceur.Lancer(0);
+    lanceur.Demeler(0);
     timer.reset();
     timer.stop();
   }

src/main/java/frc/robot/commands/ModeAuto/TournerA180.java

@@ -4,12 +4,28 @@
 
 package frc.robot.commands.ModeAuto;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import static edu.wpi.first.units.Units.*;
 
 /* 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 TournerA180 extends Command {
-  /** Creates a new TournerA180. */
-  public TournerA180() {
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new TournerAZero. */
+  public TournerA180(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,7 +35,14 @@ public class TournerA180 extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
+      drivetrain.setControl(drive.withRotationalRate(-0.5));
+    }
+    else if(pigeon2.getYaw().getValueAsDouble() >180){
+      drivetrain.setControl(drive.withRotationalRate(0.5));
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
@@ -28,6 +51,6 @@ public class TournerA180 extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return pigeon2.getYaw().getValueAsDouble() > 165  && pigeon2.getYaw().getValueAsDouble() < 195;
   }
 }

src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java

@@ -4,12 +4,28 @@
 
 package frc.robot.commands.ModeAuto;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import static edu.wpi.first.units.Units.*;
 
 /* 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 TournerAZero extends Command {
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
   /** Creates a new TournerAZero. */
-  public TournerAZero() {
+  public TournerAZero(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,7 +35,14 @@ public class TournerAZero extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
+      drivetrain.setControl(drive.withRotationalRate(0.5));
+    }
+    else if(pigeon2.getYaw().getValueAsDouble() >180){
+      drivetrain.setControl(drive.withRotationalRate(-0.5));
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
@@ -28,6 +51,6 @@ public class TournerAZero extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return pigeon2.getYaw().getValueAsDouble() > 345 || pigeon2.getYaw().getValueAsDouble() < 15;
   }
 }

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

@@ -23,8 +23,13 @@ public class MonterGrimpeur extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(grimpeur.Position() < grimpeur.PositionFinal()){
-      grimpeur.Grimper(0.5);
+    
+    if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){
+      grimpeur.Grimper(0.3);
+      System.out.println("monte");
+    }
+    else {
+      grimpeur.Grimper(0);
     }
   }
 

src/main/java/frc/robot/subsystems/Balayeuse.java

@@ -39,7 +39,7 @@ public class Balayeuse extends SubsystemBase {
     Pivot.getEncoder().setPosition(0);
   }
   public boolean GetLimiSwtich(){
-    return limit.get();
+    return !limit.get();
   }
   public double Amp(){
     return Balaye2.getOutputCurrent();

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -10,10 +10,15 @@ import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -44,74 +49,31 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
     /* Keep track if we've ever applied the operator perspective before or not */
     private boolean m_hasAppliedOperatorPerspective = false;
-
-    /* Swerve requests to apply during SysId characterization */
-    private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
-    private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
-    private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
-
-    /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
-    private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
-        new SysIdRoutine.Config(
-            null,        // Use default ramp rate (1 V/s)
-            Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
-            null,        // Use default timeout (10 s)
-            // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
-        ),
-        new SysIdRoutine.Mechanism(
-            output -> setControl(m_translationCharacterization.withVolts(output)),
-            null,
-            this
-        )
-    );
-
-    /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
-    private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
-        new SysIdRoutine.Config(
-            null,        // Use default ramp rate (1 V/s)
-            Volts.of(7), // Use dynamic voltage of 7 V
-            null,        // Use default timeout (10 s)
-            // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
-        ),
-        new SysIdRoutine.Mechanism(
-            volts -> setControl(m_steerCharacterization.withVolts(volts)),
-            null,
-            this
-        )
-    );
-
-    /*
-     * SysId routine for characterizing rotation.
-     * This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
-     * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
-     */
-    private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
-        new SysIdRoutine.Config(
-            /* This is in radians per second², but SysId only supports "volts per second" */
-            Volts.of(Math.PI / 6).per(Second),
-            /* This is in radians per second, but SysId only supports "volts" */
-            Volts.of(Math.PI),
-            null, // Use default timeout (10 s)
-            // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
-        ),
-        new SysIdRoutine.Mechanism(
-            output -> {
-                /* output is actually radians per second, but SysId only supports "volts" */
-                setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
-                /* also log the requested output for SysId */
-                SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
-            },
-            null,
-            this
-        )
-    );
-
-    /* The SysId routine to test */
-    private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
-
+    private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
+     private void configureAutoBuilder() {
+        try {
+            RobotConfig config = RobotConfig.fromGUISettings();
+            AutoBuilder.configure(
+                () -> getState().Pose,  
+                this::resetPose,         
+                () -> getState().Speeds, 
+                (speeds, feedforwards) -> setControl(
+                    m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
+                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
+                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
+                ),
+                new PPHolonomicDriveController(
+                    new PIDConstants(10, 0, 0),
+                    new PIDConstants(7, 0, 0)
+                ),
+                config,
+                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+                this 
+            );
+        } catch (Exception ex) {
+            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
+        }
+    }
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
      * <p>
@@ -130,6 +92,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
+        configureAutoBuilder();
     }
 
     /**
@@ -154,6 +117,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
+        configureAutoBuilder();
     }
 
     /**
@@ -186,6 +150,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
+        configureAutoBuilder();
     }
 
     /**
@@ -198,28 +163,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         return run(() -> this.setControl(request.get()));
     }
 
-    /**
-     * Runs the SysId Quasistatic test in the given direction for the routine
-     * specified by {@link #m_sysIdRoutineToApply}.
-     *
-     * @param direction Direction of the SysId Quasistatic test
-     * @return Command to run
-     */
-    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
-        return m_sysIdRoutineToApply.quasistatic(direction);
-    }
-
-    /**
-     * Runs the SysId Dynamic test in the given direction for the routine
-     * specified by {@link #m_sysIdRoutineToApply}.
-     *
-     * @param direction Direction of the SysId Dynamic test
-     * @return Command to run
-     */
-    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
-        return m_sysIdRoutineToApply.dynamic(direction);
-    }
-
     @Override
     public void periodic() {
         /*

src/main/java/frc/robot/subsystems/Grimpeur.java

@@ -17,11 +17,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 public class Grimpeur extends SubsystemBase {
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
   SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless);
-  SparkMax grimpeur2 = new SparkMax(4,MotorType.kBrushless);
+  SparkMax grimpeur2 = new SparkMax(12,MotorType.kBrushless);
   SparkMaxConfig slaveConfig = new SparkMaxConfig();
   DigitalInput limit = new DigitalInput(0);
     private GenericEntry EncodeurGrimpeur =
-  teb.add("Position haut grimpeur", 10).getEntry();
+  teb.add("Position haut grimpeur", 105).getEntry();
   public void Grimper(double vitesse){
     grimpeur1.set(vitesse);
     grimpeur2.set(vitesse);
@@ -36,7 +36,7 @@ public class Grimpeur extends SubsystemBase {
     return limit.get();
   }
   public double PositionFinal(){
-    return EncodeurGrimpeur.getDouble(10);
+    return EncodeurGrimpeur.getDouble(105);
   }
   /** Creates a new Grimpeur. */
   public Grimpeur() {

src/main/java/frc/robot/subsystems/Lanceur.java

@@ -18,12 +18,12 @@ public class Lanceur extends SubsystemBase {
   SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless);
   SparkFlex Demeleur = new SparkFlex(19, MotorType.kBrushless);
   GenericEntry vitesse = 
-  teb.add("vitesse lanceur",100).getEntry();
+  teb.add("vitesse lanceur",0.5).getEntry();
   GenericEntry AmpLanceur = 
   teb.add("ampérage lanceur",30).getEntry();
   public void Lancer(double vitesse){
-    moteur1.set(vitesse);
-    moteur2.set(-vitesse);
+    // moteur1.set(-vitesse);
+    moteur2.set(vitesse);
   }
   public void Demeler(double vitesse){
     Demeleur.set(vitesse);
@@ -38,7 +38,7 @@ public class Lanceur extends SubsystemBase {
     return AmpLanceur.getDouble(30);
   }
   public double vitesseDemander(){
-    return vitesse.getDouble(100);
+    return vitesse.getDouble(0.5);
   }
   /** Creates a new Lanceur. */
   public Lanceur() {