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 1/2] 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 e97338f9..fc3e7a7d 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 a85a8f1b..e15b3b59 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 a642f3c5..d2ac4caa 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. */

From 60b432dc5afc8273be33450ee947e589b9d1c5a7 Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Thu, 28 Mar 2024 17:26:09 -0400
Subject: [PATCH 2/2] made note align run forever

---
 src/main/java/frc/robot/commands/auto/NoteAlign.java  | 8 +++++++-
 src/main/java/frc/robot/commands/leds/DefaultLED.java | 1 +
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/auto/NoteAlign.java b/src/main/java/frc/robot/commands/auto/NoteAlign.java
index da81d89d..f99d2575 100644
--- a/src/main/java/frc/robot/commands/auto/NoteAlign.java
+++ b/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;
   }
 }
diff --git a/src/main/java/frc/robot/commands/leds/DefaultLED.java b/src/main/java/frc/robot/commands/leds/DefaultLED.java
index fc3e7a7d..a9077a9b 100644
--- a/src/main/java/frc/robot/commands/leds/DefaultLED.java
+++ b/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;