added LEDs

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

@@ -7,16 +7,21 @@ package frc.robot;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+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.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.ArmConstants;
 import frc.robot.constants.IOConstants;
+import frc.robot.constants.LEDConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Grabber;
 import frc.robot.subsystems.HighFive;
+import frc.robot.subsystems.LEDs;
 
 public class RobotContainer {
     private CommandPS5Controller driver;
@@ -24,6 +29,8 @@ public class RobotContainer {
     private DriveTrain driveTrain;
     private Grabber grabber;
     private HighFive highFive;
+    private LEDs leds;
+    
 
     public RobotContainer() {
         arm = new Arm();
@@ -31,6 +38,7 @@ public class RobotContainer {
         grabber = new Grabber(); 
         driver = new CommandPS5Controller(IOConstants.kDriverID0);
         highFive = new HighFive();
+        leds = new LEDs(LEDConstants.kPWMPortID, LEDConstants.kNumLEDs);
         configureBindings();
     }
 
@@ -54,6 +62,13 @@ public class RobotContainer {
 
         driver.povDown().whileTrue(arm.runArmMotor(-1));
 
+        Trigger highFiveTrigger = new Trigger(highFive::SwitchDigitalInput);
+
+        highFiveTrigger.whileTrue(
+            new RunCommand(() -> leds.setAll(Color.kRed), leds)
+        );
+       
+
         Shuffleboard.getTab("HighFive")
         .addBoolean("Switch Pressed", () -> highFive.SwitchDigitalInput());
     }

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

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class LEDConstants {
+    public static final int kPWMPortID = 0;
+    public static final int kNumLEDs = 256;
+}

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

@@ -0,0 +1,58 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+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.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+public class LEDs extends SubsystemBase {
+    private AddressableLED leds;
+    private AddressableLEDBuffer buffer;
+
+    private int fluidColorIndex;
+
+    public LEDs(int pwmPort, int numLEDs) {
+        buffer = new AddressableLEDBuffer(numLEDs);
+        leds = new AddressableLED(pwmPort);
+        leds.setLength(numLEDs);
+        leds.setData(buffer);
+        leds.start();
+
+        fluidColorIndex = 0;
+    }
+
+    public Command fluidColor(double delayTime) {
+        return Commands.repeatingSequence(
+            runOnce(() -> {
+                for(int i = 0; i < buffer.getLength(); i++) {
+                    buffer.setHSV(i, (i + fluidColorIndex) % 180, 255, 255);
+                }
+                leds.setData(buffer);
+                fluidColorIndex++;
+            }),
+            new WaitCommand(delayTime)
+        );
+    }
+
+    public Command blinkColor(Color color, double delayTime) {
+        return Commands.repeatingSequence(
+            setAll(color),
+            new WaitCommand(delayTime),
+            setAll(Color.kBlack),
+            new WaitCommand(delayTime)
+        );
+    }
+
+    public Command setAll(Color color) {
+        return runOnce(() -> {
+            for(int i = 0; i < buffer.getLength(); i++) {
+                buffer.setLED(i, color);
+            }
+    
+            leds.setData(buffer);
+        });
+    }
+}