diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 03e8629..65a1e18 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/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){ - drivetrain.setControl(drive.withRotationalRate(1)); - } - else if(pigeonAngle>180){ + if(pigeonAngle>180){ drivetrain.setControl(drive.withRotationalRate(-1)); } + else{ + drivetrain.setControl(drive.withRotationalRate(1)); + } } } else{ diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 47cb128..f22a276 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/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(); } }