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

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

@@ -67,19 +67,21 @@ public class GrimperReservoir extends Command {
         angle = angle + 360;
       }
       if(alliance.get() == Alliance.Red){
+        pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
+        System.out.println(pigeonAngle);
       y = 6.959326;
       x = 13.57966;
       angle = 180;
       if(pigeonAngle< 190 && pigeonAngle> 170){
-        drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2)));
+        drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)));
       }
       else{
-        if(pigeonAngle>0 && pigeonAngle<180){
+        if(pigeonAngle>180){
-          drivetrain.setControl(drive.withRotationalRate(1));
-        }
-        else if(pigeonAngle>180){
           drivetrain.setControl(drive.withRotationalRate(-1));
         }
+        else{
+          drivetrain.setControl(drive.withRotationalRate(1));
+        }
       }
       }
       else{

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

@@ -19,6 +19,7 @@ import com.ctre.phoenix6.signals.RGBWColor;
  * Subsystem that controls an addressable LED strip using a CANdle.
  */
 public class LEDSubsystem extends SubsystemBase {
+    Timer _timer;
     ShuffleboardTab teb = Shuffleboard.getTab("teb");
      private GenericEntry equipe = 
     teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();  
@@ -28,17 +29,18 @@ public class LEDSubsystem extends SubsystemBase {
         m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(255, 0, 0, 0)));
     }
     public void Rouge(){
-        new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0));
+        m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0)));
     }
     public void Vert(){
-        new SolidColor(0, 80).withColor(new RGBWColor(0, 255, 0, 0));
+        m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 255, 0, 0)));
     }
     public void Noir(){
 
-        new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 0, 0));
+        m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 0, 0)));
     }
     public void Flash(boolean couleur){
         Timer timer = new Timer();
+        timer.reset();
         timer.start();
         if(timer.get() <0.5){
             if(couleur){
@@ -55,6 +57,9 @@ public class LEDSubsystem extends SubsystemBase {
     }
     public LEDSubsystem() {
         setDefaultCommand(updateLEDs());
+        _timer = new Timer();
+        _timer.reset();
+        _timer.start();
     }
     public boolean Equipe(){
         return equipe.getBoolean(false);
@@ -65,69 +70,69 @@ public class LEDSubsystem extends SubsystemBase {
      * @return Command to run
      */
     public Command updateLEDs() {
-        double temps = DriverStation.getMatchTime();
         return run(() -> {
             if(Equipe()){
-                if(temps < 30){
+                if(_timer.get() < 30){
                     Vert();
                 }
-                else if(temps < 52){
+                else if(_timer.get() < 52){
+                    
                     Bleu();
                 }
-                else if(temps < 55){
+                else if(_timer.get() < 55){
                     Flash(true);
                 }
-                else if(temps < 77){
+                else if(_timer.get() < 77){
                     Rouge();
                 }
-                else if(temps < 80){
+                else if(_timer.get()  < 80){
                     Flash(false);
                 }
-                else if(temps < 102){
+                else if(_timer.get() < 102){
                     Bleu();
                 }
-                else if(temps < 105){
+                else if(_timer.get()  < 105){
                     Flash(true);
                 }
-                else if(temps <127){
+                else if(_timer.get() <127){
                     Rouge();
                 }
-                else if(temps < 130){
+                else if(_timer.get()  < 130){
                     Flash(false);
                 }
-                else if(temps < 140){
+                else if(_timer.get() < 140){
                     Vert();
                 }
             }
             else{
-                if(temps < 30){
+                if(_timer.get() < 30){
                     Vert();
                 }
-                else if(temps < 52){
+                else if(_timer.get() < 52){
                     Rouge();
                 }
-                else if(temps < 55){
+                else if(_timer.get()  < 55){
                     Flash(false);
                 }
-                else if(temps < 77){
+                else if(_timer.get() < 77){
                     Bleu();
                 }
-                else if(temps < 80){
+                else if(_timer.get()  < 80){
                     Flash(true);
                 }
-                else if(temps < 102){
+                else if(_timer.get() < 102){
                     Rouge();
                 }
-                else if(temps < 105){
+                else if(_timer.get()  < 105){
                     Flash(false);
                 }
-                else if(temps <127){
+                else if(_timer.get() <127){
                     Bleu();
                 }
-                else if(temps < 130){
+                else if(_timer.get()  < 130){
                     Flash(true);
                 }
-                else if(temps < 140){
+                else if(_timer.get() < 140){
                     Vert();
                 }
             }