Initial Lights

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

@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.algae.IntakeAlgaeCommand;
@@ -135,7 +136,7 @@ public RobotContainer() {
     funnelSubsystem = new FunnelSubsystem(funnelIO);
 
     ledIO = new LEDIO();
-    ledSubsystem = new LEDSubsystem();
+    ledSubsystem = new LEDSubsystem(ledIO);
 
     visionSubsystem = new VisionSubsystem(driveSubsystem);
 
@@ -171,6 +172,7 @@ private void configureTelemetry() {
     elevatorSubsystem.registerWith(telemetryService);
     funnelSubsystem.registerWith(telemetryService);
     biscuitSubsystem.registerWith(telemetryService);
+    ledSubsystem.registerWith(telemetryService);
     telemetryService.start();
   }
 
@@ -427,6 +429,20 @@ public void configurePitDashboard() {
         .add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem))
         .withPosition(2, 1)
         .withSize(1, 1);
+
+    Shuffleboard.getTab("Debug")
+        .add(
+            "Toggle LED autoplacing",
+            new InstantCommand(() -> ledSubsystem.setAutoPlacing(!ledSubsystem.getAutoPlacing())))
+        .withPosition(1, 1)
+        .withSize(1, 1);
+    Shuffleboard.getTab("Debug")
+        .add(
+            "Toggle LED Current Limiting",
+            new InstantCommand(
+                () -> ledSubsystem.setCurrentLimiting(!ledSubsystem.getCurrentLimiting())))
+        .withPosition(2, 1)
+        .withSize(1, 1);
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/constants/LEDConstants.java

@@ -1,3 +1,63 @@
 package frc.robot.constants;
 
-public class LEDConstants {}
+import edu.wpi.first.wpilibj.util.Color;
+
+public class LEDConstants {
+  public static final Color invertRedGreen(Color color) {
+    return new Color(color.green, color.red, color.blue);
+  }
+
+  public static final int kLEDPort = 0;
+  // Auto/Operator = 27
+  // other = 21.5
+  public static final int kTotalStripLength = 51;
+  public static final int kBottomStripLength = 34;
+  public static final int kTopStripLength = 17;
+  public static final int kTopFirstIndex = kBottomStripLength;
+  public static final int kAlgeaEnd = kBottomStripLength / 3 + 1;
+
+  public static final int kLevelStart = kTopFirstIndex;
+  public static final int kPlaceStart = kTopFirstIndex + kTopStripLength / 3;
+  public static final int kGetAlgeaStart = kTopFirstIndex + kTopStripLength / 3 * 2;
+  public static final int kAutoPlacingStart = kBottomStripLength / 3 * 2 + 1;
+
+  public static final Color kAlmostBlack = new Color(0, 0, 1);
+
+  // Normal LED Colors
+  public static final Color kHasAlgea = invertRedGreen(Color.kTeal);
+  public static final Color kNotHasAlgea = kAlmostBlack;
+
+  public static final Color kCoralNotInRobot = invertRedGreen(Color.kOrange);
+  public static final Color kCoralInFunnel = invertRedGreen(Color.kPurple);
+  public static final Color kCoralInRobot = invertRedGreen(Color.kAntiqueWhite);
+
+  public static final Color kL1 = invertRedGreen(Color.kRed);
+  public static final Color kL2 = invertRedGreen(Color.kLightYellow);
+  public static final Color kL3 = invertRedGreen(Color.kGreen);
+  public static final Color kL4 = invertRedGreen(Color.kBlue);
+
+  public static final Color kManual = kAlmostBlack;
+  public static final Color kRight = invertRedGreen(Color.kOrangeRed);
+  public static final Color kLeft = invertRedGreen(Color.kPurple);
+
+  public static final Color kGetAlgea = invertRedGreen(Color.kTeal);
+  public static final Color kNotGetAlgea = kAlmostBlack;
+
+  public static final Color kCurrentLimiting = invertRedGreen(Color.kRed);
+
+  public static final Color kAutoPlacing = invertRedGreen(Color.kBlue);
+
+  // Climb LED Colors
+  public static final Color kWaitingForCage = invertRedGreen(Color.kRed);
+  public static final Color kHasCage = invertRedGreen(Color.kGreen);
+  public static final Color kClimbed = invertRedGreen(Color.kGoldenrod);
+  public static final Color[] kGameColors = {
+    invertRedGreen(Color.kBlack), // a dummy color
+    invertRedGreen(Color.kDarkRed),
+    invertRedGreen(Color.kDarkGreen),
+    invertRedGreen(Color.kDarkBlue),
+    invertRedGreen(Color.kPink),
+    invertRedGreen(Color.kLightGreen),
+    invertRedGreen(Color.kLightBlue)
+  };
+}

src/main/java/frc/robot/subsystems/led/LEDIO.java

@@ -1,3 +1,84 @@
 package frc.robot.subsystems.led;
 
-public class LEDIO {}
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.AddressableLEDBufferView;
+import edu.wpi.first.wpilibj.LEDPattern;
+import edu.wpi.first.wpilibj.util.Color;
+import frc.robot.constants.LEDConstants;
+
+public class LEDIO {
+  private AddressableLED ledBase;
+  private AddressableLEDBuffer led;
+  private AddressableLEDBufferView ledTop;
+
+  public LEDIO() {
+    ledBase = new AddressableLED(LEDConstants.kLEDPort);
+    ledBase.setLength(LEDConstants.kTotalStripLength);
+    ledBase.start();
+    led = new AddressableLEDBuffer(LEDConstants.kTotalStripLength);
+    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1);
+    ledBase.setData(led);
+  }
+
+  public void setPort(int port) {
+    ledBase.close();
+    ledBase = new AddressableLED(port);
+    ledBase.setLength(led.getLength());
+    ledBase.start();
+    ledBase.setData(led);
+  }
+
+  public void setLength(int length) {
+    ledBase.setLength(length);
+    led = new AddressableLEDBuffer(length);
+    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1);
+    ledBase.setData(led);
+  }
+
+  public void updateLEDs() {
+    ledBase.setData(led);
+  }
+
+  public void setLED(int index, Color color) {
+    led.setLED(index, color);
+  }
+
+  public void setLED(int index, int r, int g, int b) {
+    led.setRGB(index, r, g, b);
+  }
+
+  public Color getLED(int index) {
+    return led.getLED(index);
+  }
+
+  public void setLEDTop(int index, Color color) {
+    ledTop.setLED(index, color);
+  }
+
+  public void setLEDTop(int index, int r, int g, int b) {
+    ledTop.setRGB(index, r, g, b);
+  }
+
+  public Color getLEDTop(int index) {
+    return ledTop.getLED(index);
+  }
+
+  public void setOff() {
+    for (int i = 0; i < led.getLength(); i++) {
+      setLED(i, Color.kBlack);
+    }
+    ledBase.setData(led);
+  }
+
+  public void setStrip(LEDPattern pattern) {
+    pattern.applyTo(led);
+  }
+
+  public void setStrip(Color color) {
+    for (int i = 0; i < led.getLength(); i++) {
+      setLED(i, color);
+    }
+    ledBase.setData(led);
+  }
+}