Adding fancy lights and stuff

src/main/java/org/tvhsfrc/frc2023/robot/Constants.java

@@ -25,11 +25,6 @@ public static class OperatorConstants {
         public static final int DRIVER_CONTROLLER_PORT = 0;
     }
 
-    /** Identifiers for all Digital IO devices on the robot. */
-    public static class DIOConstants {
-        public static final int STAGE_1_LIMIT_SWITCH = 0;
-    }
-
     /** Identifiers for all the CAN devices on the robot. */
     public static class CANConstants {
         public static final int FRONT_RIGHT_STEER_ENCODER = 1;

src/main/java/org/tvhsfrc/frc2023/robot/RobotContainer.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RepeatCommand;
@@ -25,8 +26,10 @@
 import org.tvhsfrc.frc2023.robot.commands.intake.IntakeIn;
 import org.tvhsfrc.frc2023.robot.commands.intake.IntakeOut;
 import org.tvhsfrc.frc2023.robot.subsystems.ArmSubsystem;
+import org.tvhsfrc.frc2023.robot.subsystems.BlingLightsSubsystem;
 import org.tvhsfrc.frc2023.robot.subsystems.IntakeSubsystem;
 import org.tvhsfrc.frc2023.robot.subsystems.SwerveSubsystem;
+import org.tvhsfrc.frc2023.robot.subsystems.BlingLightsSubsystem.Colors;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -39,6 +42,7 @@ public class RobotContainer {
             new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
     public final ArmSubsystem arm = new ArmSubsystem();
     public final IntakeSubsystem intake = new IntakeSubsystem();
+    public final BlingLightsSubsystem leds = new BlingLightsSubsystem();
 
     // Driver controller
     private final CommandPS4Controller controller =
@@ -82,6 +86,12 @@ private void configureBindings() {
         // ------ Driving ------ //
         controller.touchpad().onTrue(new InstantCommand(swerve::zeroGyro));
 
+        CommandBase cSetRainbow = Commands.runOnce(() -> leds.setState(Colors.RAINBOW), leds);
+        CommandBase cSetGreen = Commands.runOnce(() -> leds.setState(Colors.GREEN), leds);
+        CommandBase cSetBlue = Commands.runOnce(() -> leds.setState(Colors.BLUE), leds);
+        CommandBase cSetOff = Commands.runOnce(() -> leds.setState(Colors.OFF), leds);
+
+
         RelativeRelativeDrive drive =
                 new RelativeRelativeDrive(
                         swerve,

src/main/java/org/tvhsfrc/frc2023/robot/subsystems/ArmSubsystem.java

@@ -7,15 +7,12 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.*;
 import org.tvhsfrc.frc2023.robot.Constants;
 import org.tvhsfrc.frc2023.robot.utils.RotationUtil;
 
 public class ArmSubsystem extends SubsystemBase {
-    private final DigitalInput stage1LimitSwitch =
-            new DigitalInput(Constants.DIOConstants.STAGE_1_LIMIT_SWITCH);
 
     private final CANSparkMax stage2 =
             new CANSparkMax(
@@ -52,7 +49,6 @@ public ArmSubsystem() {
     @Override
     public void periodic() {
         SmartDashboard.putBoolean("isHome", stage1Homed);
-        SmartDashboard.putBoolean("switch", stage1LimitSwitch.get());
 
         stage2.getPIDController().setReference(stage2Setpoint, CANSparkMax.ControlType.kPosition);
     }
@@ -114,7 +110,6 @@ public TrapezoidProfile.State getStage2Position() {
 
     @Override
     public void initSendable(SendableBuilder builder) {
-        builder.addBooleanProperty("Stage 1 Limit Switch", () -> !stage1LimitSwitch.get(), null);
 
         builder.addDoubleProperty(
                 "Stage 1", () -> this.getRotations().getFirst().getRotations(), null);

src/main/java/org/tvhsfrc/frc2023/robot/subsystems/BlingLightsSubsystem.java

@@ -0,0 +1,73 @@
+package org.tvhsfrc.frc2023.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class BlingLightsSubsystem extends SubsystemBase {
+    public enum Colors {
+        GREEN,
+        BLUE,
+        RAINBOW,
+        OFF
+    }
+
+    // variables go here
+    Colors state = Colors.OFF;
+
+    int m_rainbowFirstPixelHue;
+
+    AddressableLED m_led = new AddressableLED(0);
+    // Length is expensive to set, so only set it once, then just update data
+    AddressableLEDBuffer m_ledBuffer = new AddressableLEDBuffer(36);
+
+    public BlingLightsSubsystem() {
+        m_led.setLength(m_ledBuffer.getLength());
+
+        m_led.setData(m_ledBuffer);
+        m_led.start();
+    }
+
+    private void rainbow(int m_rainbowFirstPixelHue) {
+        for (var i = 0; i < m_ledBuffer.getLength(); i++) {
+            final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180;
+            m_ledBuffer.setHSV(i, hue, 255, 128);
+        }
+        m_rainbowFirstPixelHue += 3;
+        m_rainbowFirstPixelHue %= 180;
+    }
+
+    private void rgbPicker(int int1, int int2, int int3) {
+        for (var i = 0; i < m_ledBuffer.getLength(); i++) {
+            m_ledBuffer.setRGB(i, int1, int2, int3);
+        }
+    }
+
+    @Override
+    public void periodic() {
+        switch (state) {
+            default:
+                break;
+            case GREEN:
+                rgbPicker(0, 255, 32);
+                break;
+            case BLUE:
+                rgbPicker(19, 0, 255);
+                break;
+            case RAINBOW:
+                rainbow(1);
+                break;
+            case OFF:
+                break;
+        }
+    }
+
+    // Getter and Setter
+    public Colors getState() {
+        return state;
+    }
+
+    public void setState(Colors state) {
+        this.state = state;
+    }
+}