Adds new LEDInstruction for the robot in the disabled state. (#93)

src/main/java/com/stuypulse/robot/Robot.java

@@ -15,6 +15,7 @@
 import com.stuypulse.robot.constants.Settings.RobotType;
 import com.stuypulse.robot.constants.Settings.Amper.Lift;
 import com.stuypulse.robot.constants.Settings.Amper.Score;
+import com.stuypulse.robot.subsystems.leds.instructions.LED694;
 import com.stuypulse.robot.subsystems.leds.instructions.LEDAlign;
 import com.stuypulse.robot.subsystems.leds.instructions.LEDAutonChooser;
 import com.stuypulse.robot.subsystems.leds.instructions.LEDRainbow;
@@ -90,7 +91,7 @@ public void disabledInit() {
         robot.intake.setIdleMode(IdleMode.kCoast);
         robot.conveyor.setIdleMode(IdleMode.kCoast);
 
-        scheduler.schedule(new LEDSet(new LEDRainbow()));
+        scheduler.schedule(new LEDSet(new LED694()));
 
         SmartDashboard.putString("Robot State", "DISABLED");
     }

src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LED694.java

@@ -0,0 +1,55 @@
+/************************ PROJECT IZZI *************************/
+/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.subsystems.leds.instructions;
+
+import java.util.ArrayList;
+
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.util.SLColor;
+import com.stuypulse.stuylib.util.StopWatch;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+
+public class LED694 implements LEDInstruction {
+    private SLColor[] colorMap = {SLColor.RED, SLColor.WHITE};
+    private ArrayList<Integer> colorArray = initColors();
+    private StopWatch stopWatch = new StopWatch();
+
+    @Override
+    public void setLED(AddressableLEDBuffer ledBuffer) {
+        double pulseTime = 0.01;
+
+
+        for (int i = 0; i < ledBuffer.getLength(); i++) {
+            ledBuffer.setLED(i, colorMap[colorArray.get(i)]);
+        }
+
+        if (pulseTime < stopWatch.getTime()){
+            int firstColor = colorArray.get(0);
+            for (int i = 1; i < colorArray.size(); i++){
+                colorArray.set(i - 1, colorArray.get(i));
+            }
+            colorArray.set(colorArray.size() - 1, firstColor);
+
+            stopWatch.reset();
+        }
+    }
+
+
+    private static ArrayList<Integer> initColors() {
+        ArrayList<Integer> colors = new ArrayList<>();
+        for (int i = 0; i < 6; i++) colors.add(0);
+        for (int i = 0; i < 6; i++) colors.add(1);
+        for (int i = 0; i < 9; i++) colors.add(0);
+        for (int i = 0; i < 9; i++) colors.add(1);
+        for (int i = 0; i < 4; i++) colors.add(0);
+        for (int i = 0; i < 4; i++) colors.add(1);
+        return colors;
+    }
+
+}