Twinkler plus removing the need for a controller when running in simulation

src/main/java/frc/robot/RobotContainer.java

@@ -4,32 +4,55 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.LEDConstants;
-import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.LEDs;
 
 public class RobotContainer {
   private LEDs leds;
 
-  private CommandXboxController driver;
+  private DigitalInput allRed;
+  private DigitalInput blinkOrange;
+  private DigitalInput blinkGreen;
+  private DigitalInput fluidColor;
+  private DigitalInput twinkle;
 
   public RobotContainer() {
     leds = new LEDs(LEDConstants.kPWMPortID, LEDConstants.kNumLEDs);
 
-    driver = new CommandXboxController(OIConstants.kDriverUSB);
+    allRed = new DigitalInput(0);
+    blinkOrange = new DigitalInput(1);
+    blinkGreen = new DigitalInput(2);
+    fluidColor = new DigitalInput(3);
+    twinkle = new DigitalInput(4);
 
     configureBindings();
   }
 
   private void configureBindings() {
-    driver.a().onTrue(leds.setAll(Color.kRed));
+    new Trigger(allRed::get).onTrue(
-    driver.b().onTrue(leds.blinkColor(Color.kOrange, .5));
+      leds.setAll(Color.kRed)
-    driver.x().onTrue(leds.blinkColor(Color.kLimeGreen, .5));
+    );
-    driver.y().onTrue(leds.fluidColor(.02));
+
+    new Trigger(blinkOrange::get).onTrue(
+      leds.blinkColor(Color.kOrange, .5)
+    );
+    
+    new Trigger(blinkGreen::get).onTrue(
+      leds.blinkColor(Color.kLimeGreen, .5)
+    );
+
+    new Trigger(fluidColor::get).onTrue(
+      leds.fluidColor(.02)
+    );
+    
+    new Trigger(twinkle::get).onTrue(
+      leds.twinkle(.02, LEDConstants.kNumLEDs / 8)
+    );
 
   }
 

src/main/java/frc/robot/subsystems/LEDs.java

@@ -1,5 +1,8 @@
 package frc.robot.subsystems;
 
+import java.util.ArrayList;
+import java.util.Random;
+
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.util.Color;
@@ -12,9 +15,13 @@ public class LEDs extends SubsystemBase {
     private AddressableLED leds;
     private AddressableLEDBuffer buffer;
 
+    private Random random;
+
     private int fluidColorIndex;
 
     public LEDs(int pwmPort, int numLEDs) {
+        random = new Random();
+
         buffer = new AddressableLEDBuffer(numLEDs);
         leds = new AddressableLED(pwmPort);
         leds.setLength(numLEDs);
@@ -24,6 +31,50 @@ public class LEDs extends SubsystemBase {
         fluidColorIndex = 0;
     }
 
+    public Command twinkle(double delayTime, int maxTwinkles) {
+        ArrayList<Twinkler> twinkles = new ArrayList<Twinkler>();
+
+        for(int i = 0; i < maxTwinkles; i++) {
+            twinkles.add(new Twinkler(
+                random.nextInt(180), 
+                Twinkler.getNextUnusedIndex(
+                    random, 
+                    buffer.getLength(), 
+                    twinkles.stream().filter(Twinkler::isRunning).mapToInt(Twinkler::getIndex).toArray()
+                ),
+                Twinkler.kAllowedSpeeds[random.nextInt(Twinkler.kAllowedSpeeds.length)],
+                random.nextBoolean()
+            ));
+        }
+
+        return setAll(Color.kBlack).andThen(
+            Commands.repeatingSequence(
+                runOnce(() -> {
+                    for(Twinkler twinkler: twinkles) {
+                        if (twinkler.isRunning()) {
+                            twinkler.update(buffer);
+                        } else {
+                            if(random.nextBoolean()) {
+                                twinkler.reset(
+                                    random.nextInt(180), 
+                                    Twinkler.getNextUnusedIndex(
+                                        random, 
+                                        buffer.getLength(), 
+                                        twinkles.stream().filter(Twinkler::isRunning).mapToInt(Twinkler::getIndex).toArray()
+                                    ),
+                                    Twinkler.kAllowedSpeeds[random.nextInt(Twinkler.kAllowedSpeeds.length)]
+                                );
+                            }
+                        }
+                    }
+
+                    leds.setData(buffer);
+                }),
+                new WaitCommand(delayTime)
+            )
+        );  
+    }
+
     public Command fluidColor(double delayTime) {
         return Commands.repeatingSequence(
             runOnce(() -> {
@@ -55,4 +106,70 @@ public class LEDs extends SubsystemBase {
             leds.setData(buffer);
         });
     }
+
+    private class Twinkler {
+        private int hue;
+        private int index;
+        private int updateStep;
+        private int updateDirection;
+
+        private boolean isRunning;
+
+        public static final int[] kAllowedSpeeds = {4, 8, 16, 32};
+
+        public Twinkler(int hue, int index, int changeSpeed, boolean shouldRun) {
+            reset(hue, index, changeSpeed);
+
+            isRunning = shouldRun;
+        }
+
+        public int getIndex() {
+            return index;
+        }
+
+        public boolean isRunning() {
+            return isRunning;
+        }
+
+        public void reset(int hue, int index, int changeSpeed) {
+            this.hue = hue;
+            this.index = index;
+
+            isRunning = true;
+            updateStep = 1;
+            updateDirection = changeSpeed;
+        }
+
+        public void update(AddressableLEDBuffer buffer) {
+            buffer.setHSV(index, hue, 255, updateStep);
+
+            updateStep += updateDirection;
+
+            if(updateStep > 255) {
+                updateStep -= updateDirection;
+                updateDirection *= -1;
+            } else if(updateStep < 1) {
+                isRunning = false;
+            }
+        }
+
+        public static int getNextUnusedIndex(Random random, int stripLength, int[] allIndexesInUse) {
+            // TODO Probably should timeout
+            int nextTry;
+
+            while (true) {
+                nextTry = random.nextInt(stripLength);
+
+                for(int i: allIndexesInUse) {
+                    if (i == nextTry) {
+                        continue;
+                    }
+                }
+
+                break;
+            }
+
+            return nextTry;
+        }
+    }
 }