Merge pull request #8 from SciBorgs/led

LEDS!

simgui.json

@@ -1,5 +1,13 @@
 {
   "HALProvider": {
+    "Addressable LEDs": {
+      "0": {
+        "columns": 64
+      },
+      "window": {
+        "visible": true
+      }
+    },
     "Other Devices": {
       "window": {
         "visible": false
@@ -190,7 +198,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 278,
+          "height": 140,
           "series": [
             {
               "color": [

src/main/java/org/sciborgs1155/robot/Ports.java

@@ -31,6 +31,10 @@ public static final class Scoral {
     public static final int BEAMBREAK = 23;
   }
 
+  public static final class LEDs {
+    public static final int LED_PORT = 1;
+  }
+
   public static final class Hopper {
     public static final int LEFT_MOTOR = -1;
     public static final int RIGHT_MOTOR = -1;

src/main/java/org/sciborgs1155/robot/Robot.java

@@ -33,6 +33,7 @@
 import org.sciborgs1155.robot.commands.Autos;
 import org.sciborgs1155.robot.drive.Drive;
 import org.sciborgs1155.robot.elevator.Elevator;
+import org.sciborgs1155.robot.led.LEDStrip;
 import org.sciborgs1155.robot.vision.Vision;
 
 /**
@@ -51,6 +52,7 @@ public class Robot extends CommandRobot implements Logged {
   // SUBSYSTEMS
   private final Drive drive = Drive.create();
   private final Vision vision = Vision.create();
+  private final LEDStrip led = new LEDStrip();
   private final Elevator elevator = Elevator.create();
 
   // COMMANDS
@@ -134,8 +136,10 @@ private void configureBindings() {
             .rateLimit(MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second)));
 
     drive.setDefaultCommand(drive.drive(x, y, omega));
+    led.setDefaultCommand(led.scrolling());
 
     autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy());
+    autonomous().whileTrue(led.autos());
 
     test().whileTrue(systemsCheck());
 

src/main/java/org/sciborgs1155/robot/led/LEDConstants.java

@@ -0,0 +1,12 @@
+package org.sciborgs1155.robot.led;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.units.measure.Distance;
+
+public class LEDConstants {
+  // The length of the LED Strip.
+  public static final int LED_LENGTH = 64;
+  // The distance between two LEDs on the LED Strip.
+  public static final Distance LED_SPACING = Meters.of(0.01);
+}

src/main/java/org/sciborgs1155/robot/led/LEDStrip.java

@@ -0,0 +1,96 @@
+package org.sciborgs1155.robot.led;
+
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static org.sciborgs1155.robot.Ports.LEDs.*;
+import static org.sciborgs1155.robot.led.LEDConstants.*;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.LEDPattern;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.Map;
+import java.util.function.DoubleSupplier;
+import monologue.Logged;
+import org.sciborgs1155.robot.Constants;
+
+public class LEDStrip extends SubsystemBase implements Logged, AutoCloseable {
+
+  private final AddressableLED led = new AddressableLED(LED_PORT);
+  private final AddressableLEDBuffer buffer;
+
+  public LEDStrip() {
+    led.setLength(LED_LENGTH);
+    buffer = new AddressableLEDBuffer(LED_LENGTH);
+    led.setData(buffer);
+    led.start();
+  }
+
+  /** Rainbow LEDs, scrolling at 0.5 m/s. Very cool. */
+  public Command rainbow() {
+    return set(
+        LEDPattern.rainbow(225, 225).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING));
+  }
+
+  /**
+   * A gradient of green to yellow LEDs, with an applied mask of how much the elevator is raised.
+   *
+   * @param percent A double supplier that supplies the elevator's percent raised.
+   */
+  public Command elevatorLED(DoubleSupplier percent) {
+    return set(
+        LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow)
+            .mask(LEDPattern.progressMaskLayer(percent)));
+  }
+
+  /** A gradient of green to yellow LEDs, moving at 60 bpm, which synchronizes with many song. */
+  public Command music() {
+    return set(
+        LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow)
+            .mask(
+                LEDPattern.progressMaskLayer(
+                    () ->
+                        Math.sin(RobotController.getMeasureTime().in(Seconds) * Math.PI * 2) / 3
+                            + 0.5)));
+  }
+
+  /** A solid yellow green that is scrolled through. */
+  public Command scrolling() {
+    return set(
+        LEDPattern.solid(Color.kGreenYellow)
+            .mask(
+                LEDPattern.steps(Map.of(0, Color.kWhite, 0.5, Color.kBlack))
+                    .scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING)));
+  }
+
+  /** A breathing gradient that matches the alliance colors. */
+  public Command autos() {
+    if (Constants.alliance() == Alliance.Red) {
+      return set(
+          LEDPattern.gradient(
+                  LEDPattern.GradientType.kDiscontinuous, Color.kFirstRed, Color.kOrangeRed)
+              .breathe(Seconds.of(2)));
+    } else {
+      return set(
+          LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kFirstBlue, Color.kAqua)
+              .breathe(Seconds.of(2)));
+    }
+  }
+
+  public Command set(LEDPattern pattern) {
+    return run(
+        () -> {
+          pattern.applyTo(buffer);
+          led.setData(buffer);
+        });
+  }
+
+  @Override
+  public void close() throws Exception {
+    led.close();
+  }
+}