From b8210d2a5e0b4a1055efae03e153120926d6d855 Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Thu, 28 Mar 2024 17:16:16 -0400
Subject: [PATCH] Break LEDs

---
 src/main/java/frc/robot/commands/leds/DefaultLED.java    | 4 ++++
 src/main/java/frc/robot/commands/swerve/BrakeSwerve.java | 6 +++++-
 src/main/java/frc/robot/subsystems/LEDSubsystem.java     | 3 ++-
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/leds/DefaultLED.java b/src/main/java/frc/robot/commands/leds/DefaultLED.java
index e97338f..fc3e7a7 100644
--- a/src/main/java/frc/robot/commands/leds/DefaultLED.java
+++ b/src/main/java/frc/robot/commands/leds/DefaultLED.java
@@ -55,6 +55,10 @@ public void execute() {
 
       default:
         break;
+      
+      case BRAKE:
+        _leds.blink(Constants.LEDColors.RED, Constants.LEDColors.NOTHING, 0.1);
+        break;
     }
   }
 
diff --git a/src/main/java/frc/robot/commands/swerve/BrakeSwerve.java b/src/main/java/frc/robot/commands/swerve/BrakeSwerve.java
index a85a8f1..e15b3b5 100644
--- a/src/main/java/frc/robot/commands/swerve/BrakeSwerve.java
+++ b/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.
diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java
index a642f3c..d2ac4ca 100644
--- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java
+++ b/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. */