diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18234bc..73ecb9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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()); } diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java new file mode 100644 index 0000000..6562d81 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java new file mode 100644 index 0000000..fecf99c --- /dev/null +++ b/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); + }); + } +} \ No newline at end of file