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

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

@@ -48,7 +48,9 @@ public class Lancer extends Command {
   public void execute() {
     double[] BotPose = new double[6];
     if(limeLight3G.getV()){
-    if(!alliance.isPresent()){return;}
+    if(!alliance.isPresent()){
+      return;
+    }
       if(alliance.get() == Alliance.Blue){
         BotPose = limeLight3G.getBotPoseBlue();
       }
@@ -60,10 +62,11 @@ public class Lancer extends Command {
     vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
   }                                            
     if(limeLight3G.getV()){
-      System.out.println(vitesse);
+      
 
       double output = pidController.calculate(lanceur.Vitesse(),vitesse);
       lanceur.Lancer(output);
+      System.out.println(output);
       if(lanceur.Vitesse() >= vitesse-800){
         timer.start();
         if(timer.get() >1){

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

@@ -32,6 +32,7 @@ public class GrimperReservoir extends Command {
   double x;
   double y;
   double angle;
+  double pigeonAngle;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
     Optional<Alliance> alliance = DriverStation.getAlliance(); 
@@ -59,11 +60,11 @@ public class GrimperReservoir extends Command {
   @Override
   public void execute() {
     if(limeLight3G.getV()){
-
       BotPose = limeLight3G.getBotPoseBlue();
       botx = BotPose[0];
       boty = BotPose[1];
-      System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
+      pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
+      System.out.println(pigeonAngle);
       if(angle < 0){
         angle = angle + 360;
       }
@@ -71,14 +72,14 @@ public class GrimperReservoir extends Command {
       y = 6.959326;
       x = 13.57966;
       angle = 180;
-      if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){
+      if(pigeonAngle< 190 && pigeonAngle> 170){
         drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2)));
       }
       else{
-        if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
+        if(pigeonAngle>0 && pigeonAngle<180){
           drivetrain.setControl(drive.withRotationalRate(1));
         }
-        else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
+        else if(pigeonAngle>180){
           drivetrain.setControl(drive.withRotationalRate(-1));
         }
       }
@@ -87,15 +88,15 @@ public class GrimperReservoir extends Command {
         y = 2.6;
       x = 1.11;
       angle = 0;
-      if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 358 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 2){
+      if(pigeonAngle> 358 || pigeonAngle< 2){
         drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0));
       }
       else{
-        if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
+        if(pigeonAngle>0 && pigeonAngle<180){
           drivetrain.setControl(drive.withRotationalRate(-1));
           System.out.println("x");
         }
-        else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
+        else if(pigeonAngle>180){
           System.out.println("e");
           drivetrain.setControl(drive.withRotationalRate(1));
         }