LED patterns

src/main/java/team1403/robot/Constants.java

@@ -203,4 +203,9 @@ public static class AlgaeWrist {
     public static final double Ki = 0.0;
     public static final double Kd = 0.0;
   }
+
+  public static class LED {
+    public static final double speed = 0.7;
+    public static final int kLedCount = 0; //Placeholder
+  }
 }

src/main/java/team1403/robot/RobotContainer.java

@@ -337,7 +337,7 @@ private void configureBindings() {
     ).withTimeout(2)); */
 
     m_coralIntake.setDefaultCommand(new DefaultIntakeCommand(m_coralIntake));
-    m_led.setDefaultCommand(new LightCommand(m_led));
+    m_led.setDefaultCommand(new LightCommand(m_led, m_elevator).ignoringDisable(true));
     // m_algaeIntake.setDefaultCommand(new DefaultAlgaeIntakeCommand(m_algaeIntake));
 
     // coral intake

src/main/java/team1403/robot/commands/LightCommand.java

@@ -1,21 +1,26 @@
 package team1403.robot.commands;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import team1403.robot.subsystems.Blackbox;
+import team1403.robot.subsystems.ElevatorSubsystem;
 import team1403.robot.subsystems.LEDSubsystem;
+import team1403.robot.subsystems.LEDSubsystem.LEDConfig;
 
 public class LightCommand extends Command{
 
     //Variable for local copy of LED subsystem
     private LEDSubsystem m_LED;
+    private ElevatorSubsystem m_elevator;
 
     /***
      * Runs the lights based of robot state
      * 
      * @param LED LED subsystem
      */
-    public LightCommand(LEDSubsystem LED) {
+    public LightCommand(LEDSubsystem LED, ElevatorSubsystem elevator) {
         m_LED = LED;
+        m_elevator = elevator;
 
         addRequirements(m_LED);
     }
@@ -24,28 +29,49 @@ public LightCommand(LEDSubsystem LED) {
     public void initialize() {}
 
     @Override
+    //Runs every 20 milliseconds
     public void execute() {
-        switch(Blackbox.robotState){
-            case loading:
-                m_LED.setLEDcolor(LEDSubsystem.Color.Yellow);
-            break;
-            case driving:
-                m_LED.setLEDcolor(LEDSubsystem.Color.Green);
-            break;
-            case aligning:
-                m_LED.setLEDcolor(LEDSubsystem.Color.Blue);
-            break;
-            case placing:
-                m_LED.setLEDcolor(LEDSubsystem.Color.Pink);
-            break;
-            case exiting:
-                m_LED.setLEDcolor(LEDSubsystem.Color.Red);
-            break;
-            case ManualElevator:
-                m_LED.setLEDcolor(LEDSubsystem.Color.White);
-            break;
+    if(true){
+        if(!m_elevator.isAtSetpoint()) {
+            if(m_elevator.isGoingUp){
+                m_LED.setLEDcolor(LEDConfig.Style.Upwards, LEDConfig.Color.Green);
+            }
+            else if(!m_elevator.isGoingUp) {
+                m_LED.setLEDcolor(LEDConfig.Style.Downwards, LEDConfig.Color.Grey);
+            }
         }
 
+        else if(DriverStation.isDisabled()) {
+            m_LED.setLEDcolor(LEDConfig.Style.Strobe, LEDConfig.Color.White);
+        }
+
+        else if(DriverStation.isEStopped()) {
+            m_LED.setLEDcolor(LEDConfig.Style.Strobe, LEDConfig.Color.Red);
+        }
+
+        else {
+            switch(Blackbox.robotState){
+                case loading:
+                    m_LED.setLEDcolor(LEDConfig.Color.Yellow);
+                break;
+                case driving:
+                    m_LED.setLEDcolor(LEDConfig.Color.Green);
+                break;
+                case aligning:
+                    m_LED.setLEDcolor(LEDConfig.Color.Blue);
+                break;
+                case placing:
+                    m_LED.setLEDcolor(LEDConfig.Color.Pink);
+                break;
+                case exiting:
+                    m_LED.setLEDcolor(LEDConfig.Color.Red);
+                break;
+                case ManualElevator:
+                    m_LED.setLEDcolor(LEDConfig.Color.White);
+                break;
+            }
+        }
+    }
     }
 
     @Override