From c0ed3bc97dada8ff128ec9995893133bc877ba30 Mon Sep 17 00:00:00 2001
From: JCasey7135 <126344360+Rachel-M-Greenlee@users.noreply.github.com>
Date: Mon, 24 Feb 2025 18:52:29 -0500
Subject: [PATCH] Implemented progressive LEDs on elevator and made a getter
 for Max Elevator Height

---
 .../frc/robot/subsystems/elevator/Elevator.java   |  4 ++++
 src/main/java/frc/robot/subsystems/leds/LEDs.java | 15 +++++++++++++--
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
index 8c71a38..d5cb940 100644
--- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -190,6 +190,10 @@ public void setCurrentMode(GamepieceMode mode) {
     _currentMode = mode;
   }
 
+  public double getElevatorMaxHeight() {
+    return ELEVATOR_MAX_INCHES;
+  }
+
   public void populateLog(SysIdRoutineLog log) {
     log.motor("elevator_primary")
         .voltage(Voltage.ofBaseUnits(_inputs._elevatorMotorVoltage, Volt))
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java
index aa06b04..11da210 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDs.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java
@@ -6,9 +6,11 @@
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.HardwareConstants;
 import frc.robot.HardwareConstants.CAN;
+import frc.robot.subsystems.elevator.Elevator;
 
 import com.ctre.phoenix.led.RainbowAnimation;
 import com.ctre.phoenix.led.StrobeAnimation;
@@ -20,10 +22,12 @@
 public class LEDs extends SubsystemBase {
 
     CANdle _candle = new CANdle(CAN.CANDLE_ID);
-    static int _numLEDs = 8;
+    static int _numLEDs = 60; //leds/height * currentheight 
     boolean _alreadyRunning = false;
     LEDAnimation _currentAnimation = LEDAnimation.None;
 
+    Elevator _elevator;
+
     public enum LEDAnimation {
         None(null, null, 0),
 
@@ -43,7 +47,9 @@ public enum LEDAnimation {
 
         SolidTeal(new LEDColor(0, 225, 174), null, 0),
 
-        SolidCoral(new LEDColor(255, 80, 15), null, 0);
+        SolidCoral(new LEDColor(255, 80, 15), null, 0),
+
+        SolidRed(new LEDColor(255, 0, 0), null, 0);
 
         LEDColor _color;
         Animation _animation;
@@ -81,6 +87,11 @@ public void runAnimation(LEDAnimation animation) {
                 _candle.clearAnimation(0);
                 _candle.setLEDs(0, 0, 0);
                 _candle.animate(animation.getAnimation());
+            } else if (animation == LEDAnimation.SolidRed) {
+                _candle.clearAnimation(0);
+                LEDColor color = animation.getColor();
+                _candle.setLEDs(color.getR(), color.getG(), color.getB(),0, 0, 
+                (int) Math.round(_numLEDs/_elevator.getElevatorMaxHeight() * _elevator.getCurrentPosInches()));
             } else if (animation.getAnimation() == null) {
                 LEDColor color = animation.getColor();
                 _candle.clearAnimation(0);