Merge branch 'lights' of https://github.com/strykeforce/reefscape into lights

Author: PotatoBoyH4

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

@@ -3,56 +3,61 @@
 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 = 70;
-  public static final int kBottomStripLength = 43;
-  public static final int kTopStripLength = 27;
-  public static final int kTopFirstIndex = kBottomStripLength + 1;
-
+  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 = kTopFirstIndex;
+  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 = Color.kAquamarine;
+  public static final Color kHasAlgea = invertRedGreen(Color.kTeal);
   public static final Color kNotHasAlgea = kAlmostBlack;
 
-  public static final Color kCoralNotInRobot = Color.kOrange;
-  public static final Color kCoralInFunnel = Color.kHotPink;
-  public static final Color kCoralInRobot = Color.kWhite;
+  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 = Color.kBlue;
-  public static final Color kL2 = Color.kGreen;
-  public static final Color kL3 = Color.kYellow;
-  public static final Color kL4 = Color.kRed;
+  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 = Color.kSaddleBrown;
-  public static final Color kLeft = Color.kGreenYellow;
+  public static final Color kRight = invertRedGreen(Color.kOrangeRed);
+  public static final Color kLeft = invertRedGreen(Color.kPurple);
 
-  public static final Color kGetAlgea = Color.kAquamarine;
+  public static final Color kGetAlgea = invertRedGreen(Color.kTeal);
   public static final Color kNotGetAlgea = kAlmostBlack;
 
-  public static final Color kCurrentLimiting = Color.kRed;
+  public static final Color kCurrentLimiting = invertRedGreen(Color.kRed);
 
-  public static final Color kAutoPlacing = Color.kPurple;
+  public static final Color kAutoPlacing = invertRedGreen(Color.kBlue);
 
   // Climb LED Colors
-  public static final Color kWaitingForCage = Color.kRed;
-  public static final Color kHasCage = Color.kGreen;
-  public static final Color kClimbed = Color.kGoldenrod;
+  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 = {
-    Color.kBlack, // a dummy color
-    Color.kDarkRed,
-    Color.kDarkGreen,
-    Color.kDarkBlue,
-    Color.kPink,
-    Color.kLightGreen,
-    Color.kLightBlue
+    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

@@ -12,12 +12,12 @@ public class LEDIO {
   private AddressableLEDBuffer led;
   private AddressableLEDBufferView ledTop;
 
-  public LEDIO(int port, int length) {
-    ledBase = new AddressableLED(port);
-    ledBase.setLength(length);
+  public LEDIO() {
+    ledBase = new AddressableLED(LEDConstants.kLEDPort);
+    ledBase.setLength(LEDConstants.kTotalStripLength);
     ledBase.start();
-    led = new AddressableLEDBuffer(length);
-    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1);
+    led = new AddressableLEDBuffer(LEDConstants.kTotalStripLength);
+    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1);
     ledBase.setData(led);
   }
 
@@ -32,7 +32,7 @@ public void setPort(int port) {
   public void setLength(int length) {
     ledBase.setLength(length);
     led = new AddressableLEDBuffer(length);
-    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1);
+    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1);
     ledBase.setData(led);
   }