From 965692fae02381150870753d52d5edc816dfb579 Mon Sep 17 00:00:00 2001
From: SR1899 <programming@saintsrobotics.com>
Date: Thu, 4 Apr 2024 10:34:40 -0700
Subject: [PATCH] disco

---
 .../frc/robot/commands/DefaultLEDCommand.java |  6 ++++
 .../frc/robot/subsystems/LEDSubsystem.java    | 32 +++++++++++++++++--
 2 files changed, 36 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java
index 727b663..0bc5dc8 100644
--- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java
+++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java
@@ -70,6 +70,12 @@ public void initialize() {
       rgb[2] = 257;
     }
 
+    if (m_climberSubsystem.getState() == Value.kReverse){
+      rgb[0] = 258;
+      rgb[1] = 258;
+      rgb[2] = 258;
+    }
+
     if (m_intakeSubsystem.getArmPosition() == IntakeConstants.kIntakeAmpScoringAngle){
       if (m_intakeSubsystem.ampReady()){
         rgb[0] = 0;
diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java
index eb1b672..cd113b2 100644
--- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java
@@ -4,6 +4,8 @@
 
 package frc.robot.subsystems;
 
+import java.util.Random;
+
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -15,6 +17,8 @@ public class LEDSubsystem extends SubsystemBase {
   private final AddressableLEDBuffer m_LEDBuffer = new AddressableLEDBuffer(LEDConstants.kLEDLength);
   private int m_rainbowFirstPixelHue = 0;
 
+  private Random rand = new Random();
+
   /** Creates a new {@link LEDSubsystem}. */
   public LEDSubsystem() {
     m_LED.setLength(LEDConstants.kLEDLength); // 29
@@ -38,7 +42,7 @@ public void setLED(int r, int g, int b) {
 
     // If we get an invalid value just set it to a rainbow
     if (r == 256 && g == 256 && b == 256) {
-      rainbow();
+      disco();
       return;
     }
 
@@ -47,6 +51,11 @@ else if (r == 257 && g == 257 && b == 257){
       return;
     }
 
+    else if (r == 258 && g == 258 && b == 258){
+      disco();
+      return;
+    }
+
     for (var i = 0; i < LEDConstants.kLEDLength; i++) {
       m_LEDBuffer.setRGB(i, r, g, b);
     }
@@ -77,7 +86,7 @@ private void golden() {
     for (var i = 0; i < m_LEDBuffer.getLength(); i++) {
       // Calculate the hue - hue is easier for rainbows because the color
       // shape is a circle so only one value needs to precess
-      final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 120 + 90;
+      final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 120;
       // Set the value
       m_LEDBuffer.setHSV(i, hue, 240, 180);
     }
@@ -87,6 +96,25 @@ private void golden() {
     m_rainbowFirstPixelHue %= 120;
     
 
+    m_LED.setData(m_LEDBuffer);
+    SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
+  }
+
+  private void disco() {
+    // For every pixel
+    for (var i = 0; i < m_LEDBuffer.getLength(); i++) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 120;
+      // Set the value
+      m_LEDBuffer.setHSV(i, hue, rand.nextInt(50, 180), rand.nextInt(50, 180));
+    }
+    // Increase by to make the rainbow "move"
+    m_rainbowFirstPixelHue += rand.nextInt(1, 8);
+    // Check bounds
+    m_rainbowFirstPixelHue %= 120;
+    
+
     m_LED.setData(m_LEDBuffer);
     SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
   }