diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ff4043..adb44ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,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( diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index dfdb6e8..2b63054 100644 --- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java +++ b/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]); } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 3a42743..9bd6f9c 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/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) */ diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 7ab725e..eb1b672 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/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()); }