Merge branch 'LED' into amp

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

@@ -135,7 +135,7 @@ public RobotContainer() {
     configureBindings();
 
     m_ledSubsystem.setDefaultCommand(
-      new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem)
+      new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem, m_climberSubsystem)
     );
 
     m_robotDrive.setDefaultCommand(

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

@@ -6,24 +6,28 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.IntakeConstants;
+import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 
 public class DefaultLEDCommand extends Command {
   private LEDSubsystem m_ledSubsystem;
   private IntakeSubsystem m_intakeSubsystem;
   private ShooterSubsystem m_shooterSubsystem;
+  private ClimberSubsystem m_climberSubsystem;
 
   private double armSetpoint;
   private double shootSpeed;
   private int[] rgb = new int[3];
 
   /** Creates a new SetLED. */
-  public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter) {
+  public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter, ClimberSubsystem climb) {
     m_ledSubsystem = LED;
     m_intakeSubsystem = intake;
     m_shooterSubsystem = shooter;
+    m_climberSubsystem = climb;
     addRequirements(m_ledSubsystem);
   }
 
@@ -35,6 +39,7 @@ public void initialize() {
     rgb[1] = 256;
     rgb[2] = 256;
 
+    //check for arm angle & haveNote
     armSetpoint = m_intakeSubsystem.getArmPosition();
     if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) {
       rgb[0] = 255;
@@ -46,6 +51,7 @@ public void initialize() {
       rgb[2] = 255;
     }
 
+    //check for shooter spinup and override b4 value
     shootSpeed = m_shooterSubsystem.returnCurrentSpeed();
     if (shootSpeed > 500 && shootSpeed < 1899 * Math.PI) { // if charging up
       rgb[0] = 150;
@@ -57,6 +63,13 @@ public void initialize() {
       rgb[2] = 0;
     }
 
+    //check for pneumatics state and override b4 value
+    if (m_climberSubsystem.getState() == Value.kForward){
+      rgb[0] = 257;
+      rgb[1] = 257;
+      rgb[2] = 257;
+    }
+
     m_ledSubsystem.setLED(rgb[0], rgb[1], rgb[2]);
   }
 

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

@@ -62,6 +62,10 @@ public void reverse() {
     m_state = kReverse;
   }
 
+  public Value getState(){
+    return m_state;
+  }
+
   /**
    * Toggles the state of the compressor (on/off)
    */

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

@@ -37,11 +37,16 @@ public LEDSubsystem() {
   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) {
+    if (r == 256 && g == 256 && b == 256) {
       rainbow();
       return;
     }
 
+    else if (r == 257 && g == 257 && b == 257){
+      golden();
+      return;
+    }
+
     for (var i = 0; i < LEDConstants.kLEDLength; i++) {
       m_LEDBuffer.setRGB(i, r, g, b);
     }
@@ -63,6 +68,25 @@ private void rainbow() {
     // Check bounds
     m_rainbowFirstPixelHue %= 180;
 
+    m_LED.setData(m_LEDBuffer);
+    SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
+  }
+
+  private void golden() {
+    // 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())) % 120 + 90;
+      // Set the value
+      m_LEDBuffer.setHSV(i, hue, 240, 180);
+    }
+    // Increase by to make the rainbow "move"
+    m_rainbowFirstPixelHue += 1;
+    // Check bounds
+    m_rainbowFirstPixelHue %= 120;
+
+
     m_LED.setData(m_LEDBuffer);
     SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
   }