From 7b535a845fc7c04da988a6413b87f337008afe9e Mon Sep 17 00:00:00 2001
From: Antoine PerreaultE <KalcioGx53@hotmail.com>
Date: Tue, 31 Mar 2026 19:59:15 -0400
Subject: [PATCH] grimpe en rouge aussi

---
 .../commands/ModeAuto/GrimperReservoir.java   | 30 +++++++++----------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
index 0b0adf6..03e8629 100644
--- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
+++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
@@ -63,8 +63,6 @@ public class GrimperReservoir extends Command {
       BotPose = limeLight3G.getBotPoseBlue();
       botx = BotPose[0];
       boty = BotPose[1];
-      pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
-      System.out.println(pigeonAngle);
       if(angle < 0){
         angle = angle + 360;
       }
@@ -85,23 +83,25 @@ public class GrimperReservoir extends Command {
       }
       }
       else{
+        pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
+        System.out.println(pigeonAngle);
         y = 2.6;
-      x = 1.11;
-      angle = 0;
-      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(pigeonAngle>0 && pigeonAngle<180){
-          drivetrain.setControl(drive.withRotationalRate(-1));
-          System.out.println("x");
+        x = 1.11;
+        angle = 0;
+        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(pigeonAngle>180){
-          System.out.println("e");
-          drivetrain.setControl(drive.withRotationalRate(1));
+        else{
+          if(pigeonAngle>0 && pigeonAngle<180){
+            drivetrain.setControl(drive.withRotationalRate(-1));
+            System.out.println("x");
+          }
+          else if(pigeonAngle>180){
+            System.out.println("e");
+            drivetrain.setControl(drive.withRotationalRate(1));
+          }
         }
       }
-      }
     }
     else{
       drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));