LED stuff

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

@@ -29,13 +29,15 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.commands.DefaultLEDCommand;
 import frc.robot.commands.IntakeArmPositionCommand;
 import frc.robot.commands.NoteIntakeCommand;
 import frc.robot.commands.NoteOuttakeCommand;
 import frc.robot.commands.ShooterSetSpeedCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.ArmPosition;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;
@@ -54,6 +56,7 @@ public class RobotContainer {
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
   private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
+  private final LEDSubsystem m_ledSubsystem = new LEDSubsystem();
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
@@ -120,6 +123,10 @@ public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
 
+    m_ledSubsystem.setDefaultCommand(
+      new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem)
+    );
+
     m_robotDrive.setDefaultCommand(
         new RunCommand(
             () -> m_robotDrive.drive(

src/main/java/frc/robot/commands/DefaultLEDCommand.java

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class DefaultLEDCommand extends Command {
+  private LEDSubsystem m_ledSubsystem;
+  private IntakeSubsystem m_intakeSubsystem;
+  private ShooterSubsystem m_shooterSubsystem;
+
+  private double armSetpoint;
+  private double shootSpeed;
+  private int[] rgb = new int[3];
+
+  /** Creates a new SetLED. */
+  public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter) {
+    m_ledSubsystem = LED;
+    m_intakeSubsystem = intake;
+    m_shooterSubsystem = shooter;
+    addRequirements(m_ledSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    // default color is invalid so the subsystem will set it to rainbow
+    rgb[0] = 256;
+    rgb[1] = 256;
+    rgb[2] = 256;
+
+    armSetpoint = m_intakeSubsystem.getArmPosition();
+    if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) {
+      rgb[0] = 255;
+      rgb[1] = 165;
+      rgb[2] = 0;
+    } else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle) {
+      rgb[0] = 0;
+      rgb[1] = 0;
+      rgb[2] = 255;
+    }
+
+    shootSpeed = m_shooterSubsystem.returnCurrentSpeed();
+    if (shootSpeed > 500 && shootSpeed < 1899 * 2) {// if charging up
+      rgb[0] = 150;
+      rgb[1] = 150;
+      rgb[2] = 0;
+    } else if (shootSpeed > 1899 * 2) {// if the shooter is ready to shoot
+      rgb[0] = 0;
+      rgb[1] = 255;
+      rgb[2] = 0;
+    }
+
+    m_ledSubsystem.setLED(rgb[0], rgb[1], rgb[2]);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

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

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import frc.robot.Constants.LEDConstants;
 
 public class LEDSubsystem extends SubsystemBase {
@@ -34,10 +35,36 @@ public LEDSubsystem() {
    * @param b Blue 0-255
    */
   public void setLED(int r, int g, int b) {
+
+    // If we get an invalid value just set it to a rainbow
+    if (r > 255 || b > 255 || g > 255) {
+      rainbow();
+      return;
+    }
+
     for (var i = 0; i < LEDConstants.kLEDLength; i++) {
       m_LEDBuffer.setRGB(i, r, g, b);
     }
     m_LED.setData(m_LEDBuffer);
     SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
   }
+
+  private void rainbow() {
+    int m_rainbowFirstPixelHue = 0;
+    // For every pixel
+    for (var i = 0; i < m_LEDBuffer.getLength(); i++) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 180;
+      // Set the value
+      m_LEDBuffer.setHSV(i, hue, 255, 128);
+    }
+    // Increase by to make the rainbow "move"
+    m_rainbowFirstPixelHue += 3;
+    // Check bounds
+    m_rainbowFirstPixelHue %= 180;
+
+    m_LED.setData(m_LEDBuffer);
+    SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
+  }
 }