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

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

@@ -40,6 +40,7 @@ public class Lancer extends Command {
     if(limeLight3G.getV()){}
     pidController = new PIDController(0.0007, 0,0, 0.001);
     timer.reset();
+    alliance = DriverStation.getAlliance();  
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -27,6 +27,7 @@ public class Limelighter extends Command {
   double boty;
   double angle;
   double calcul;
+  double x;
    Optional<Alliance> alliance = DriverStation.getAlliance(); 
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
@@ -42,7 +43,9 @@ public class Limelighter extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    alliance = DriverStation.getAlliance();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -51,26 +54,34 @@ public class Limelighter extends Command {
     System.out.println("e");
     if (limelight3g.getV()) {
         BotPose = limelight3g.getBotPoseBlue();
-    
+       if (!alliance.isPresent()) {
+        return;
+      }
+      if (alliance.get() == Alliance.Blue) {
+        x = 4.6;
+      }
+       else {
+        x = 11.915394;
+      }
       botx = BotPose[1];
       boty = BotPose[0];
       angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
-      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
+      calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
       if(calcul < -5 && calcul > -180){
         drivetrain.setControl(
-          drive.withRotationalRate(0.5*180/Math.PI));
+          drive.withRotationalRate(0.5*(2*Math.PI)));
       }
       else if(calcul > 5 && calcul < 180){
         drivetrain.setControl(
-          drive.withRotationalRate(-0.5*180/Math.PI));
+          drive.withRotationalRate(-0.5*(2*Math.PI)));
       }
       else if(calcul >= 180){
         drivetrain.setControl(
-          drive.withRotationalRate(-0.5*180/Math.PI));
+          drive.withRotationalRate(-0.5*(2*Math.PI)));
       }
       else if(calcul <= -180){
         drivetrain.setControl(
-          drive.withRotationalRate(0.5*180/Math.PI));
+          drive.withRotationalRate(0.5*(2*Math.PI)));
       }
       else{
         drivetrain.setControl(

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

@@ -47,7 +47,9 @@ public class GrimperMur extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    alliance = DriverStation.getAlliance();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

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

@@ -47,7 +47,9 @@ public class GrimperReservoir extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    alliance = DriverStation.getAlliance();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

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

@@ -38,6 +38,7 @@ public class LancerAuto extends Command {
   public void initialize() {
     pidController = new PIDController(0.0007, 0, 0, 0.001);
     timer.reset();
+    alliance = DriverStation.getAlliance();
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -27,6 +27,7 @@ public class LimelighterAuto extends Command {
   double boty;
   double angle;
   double calcul;
+  double x;
    Optional<Alliance> alliance = DriverStation.getAlliance(); 
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
@@ -42,7 +43,9 @@ public class LimelighterAuto extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    alliance = DriverStation.getAlliance();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -50,27 +53,35 @@ public class LimelighterAuto extends Command {
     double[] BotPose = new double[6];
     System.out.println("e");
     if (limelight3g.getV()) {
+      if (!alliance.isPresent()) {
+        return;
+      }
+      if (alliance.get() == Alliance.Blue) {
+        x = 4.6;
+      }
+       else {
+        x = 11.915394;
+      }
         BotPose = limelight3g.getBotPoseBlue();
-    
       botx = BotPose[1];
       boty = BotPose[0];
       angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
-      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
+      calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
       if(calcul < -5 && calcul > -180){
         drivetrain.setControl(
-          drive.withRotationalRate(0.5*180/Math.PI));
+          drive.withRotationalRate(0.5*(2*Math.PI)));
       }
       else if(calcul > 5 && calcul < 180){
         drivetrain.setControl(
-          drive.withRotationalRate(-0.5*180/Math.PI));
+          drive.withRotationalRate(-0.5*(2*Math.PI)));
       }
       else if(calcul >= 180){
         drivetrain.setControl(
-          drive.withRotationalRate(-0.5*180/Math.PI));
+          drive.withRotationalRate(-0.5*(2*Math.PI)));
       }
       else if(calcul <= -180){
         drivetrain.setControl(
-          drive.withRotationalRate(0.5*180/Math.PI));
+          drive.withRotationalRate(0.5*(2*Math.PI)));
       }
       else{
         drivetrain.setControl(

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

@@ -37,7 +37,9 @@ public class TournerVersMur extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    alliance = DriverStation.getAlliance();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

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

@@ -37,7 +37,9 @@ public class TournerVersReservoir extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    alliance = DriverStation.getAlliance();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

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

@@ -62,10 +62,10 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
       }
       else{
         if(y1 > y2){
-          return Math.atan(90-((x2 - x1) / (y2 - y1)))* (180 / Math.PI)+270 - angle;
+          return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
         }
         else{
-          return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+180 - angle;
+          return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
         }
       }
     }