Merge branch 'main' of https://github.com/Team334/R2024

src/main/java/frc/robot/commands/auto/NoteAlign.java

@@ -53,6 +53,12 @@ public void execute() {
       noteX = noteAngles.get()[0];
     }
 
+    if (_headingController.atSetpoint()) {
+
+    } else {
+
+    }
+
     double rotationVelocity = MathUtil.clamp(
       _headingController.calculate(noteX, 0),
       -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
@@ -76,6 +82,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return _headingController.atSetpoint();
+    return false;
   }
 }

src/main/java/frc/robot/commands/leds/DefaultLED.java

@@ -42,6 +42,7 @@ public void execute() {
     switch (UtilFuncs.GetLEDs()) {
       case DEFAULT:
         _leds.setColor(UtilFuncs.GetAlliance() == Alliance.Red ? LEDColors.RED : LEDColors.BLUE);
+        // 16 side | 19 back
         // leds.rainbow();
         break;
 
@@ -55,6 +56,10 @@ public void execute() {
 
       default:
         break;
+
+      case BRAKE:
+        _leds.blink(Constants.LEDColors.RED, Constants.LEDColors.NOTHING, 0.1);
+        break;
     }
   }
 

src/main/java/frc/robot/commands/swerve/BrakeSwerve.java

@@ -8,6 +8,8 @@
 import frc.robot.Constants;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
+import frc.robot.subsystems.LEDSubsystem.LEDState;
+import frc.robot.utils.UtilFuncs;
 
 /**
  * Creates a natural brake on the swerve drive by facing the modules so they
@@ -47,6 +49,7 @@ public BrakeSwerve(SwerveDriveSubsystem swerveDrive, LEDSubsystem leds, double t
   @Override
   public void initialize() {
     _timer.start();
+    UtilFuncs.SetLEDs(LEDState.BRAKE);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -58,13 +61,14 @@ public void execute() {
         new SwerveModuleState(0, Rotation2d.fromDegrees(-45))};
 
     _swerveDrive.setStates(states);
-    _leds.blink(Constants.LEDColors.RED, Constants.LEDColors.NOTHING, 0.2); // TESTING ONLY!!!!!!!
+    // _leds.blink(Constants.LEDColors.RED, Constants.LEDColors.NOTHING, 0.2); // TESTING ONLY!!!!!!!
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     _timer.stop();
+    UtilFuncs.SetLEDs((LEDState.DEFAULT));
   }
 
   // Returns true when the command should end.

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

@@ -25,7 +25,8 @@ public class LEDSubsystem extends SubsystemBase {
 
   public enum LEDState {
     DEFAULT,
-    AIM
+    AIM,
+    BRAKE
   }
 
   /** Creates a new LEDSubsystem. */