From 382cb5be78ecb42daf3972c9973d1d7078648077 Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Mon, 29 Jul 2024 17:38:38 -0500 Subject: [PATCH] Add LEDs --- .../frc/lib/util/swerve/SwerveModuleReal.java | 2 +- src/main/java/frc/robot/Constants.java | 13 +++ src/main/java/frc/robot/RobotContainer.java | 6 + .../frc/robot/commands/FlashingLEDColor.java | 65 +++++++++++ .../frc/robot/commands/MovingColorLEDs.java | 67 +++++++++++ src/main/java/frc/robot/subsystems/LEDs.java | 110 ++++++++++++++++++ 6 files changed, 262 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/FlashingLEDColor.java create mode 100644 src/main/java/frc/robot/commands/MovingColorLEDs.java create mode 100644 src/main/java/frc/robot/subsystems/LEDs.java diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 8c8d7c6..ae855e5 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -54,7 +54,6 @@ public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, in driveMotorSelectedPosition = mDriveMotor.getPosition(); driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity(); absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition(); - mAngleMotor.restoreFactoryDefaults(); @@ -62,6 +61,7 @@ public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, in private void configAngleMotor() { /* Angle Motor Config */ + this.mAngleMotor.restoreFactoryDefaults(); /* Motor Inverts and Neutral Mode */ this.mAngleMotor.setInverted(false); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5fcd2cd..92b9d06 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.util.Color; /** * Constants file. @@ -199,6 +200,18 @@ public static final class LightsaberConstants { public static final int ls3 = 7; } + /** + * LED constants. + */ + public static final class LEDConstants { + public static final int PWM_PORT = 0; + public static final int LED_COUNT = 60; + public static final Color INTAKE_COLOR = Color.kGreen; + public static final Color INDEXER_COLOR = Color.kPurple; + public static final Color ALERT_COLOR = Color.kWhite; + } + + /** * Auto constants */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 57c1e71..aff8867 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,12 +6,15 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot.RobotRunType; +import frc.robot.commands.MovingColorLEDs; import frc.robot.commands.TeleopSwerve; +import frc.robot.subsystems.LEDs; import frc.robot.subsystems.lightsabers.Lightsaber; import frc.robot.subsystems.lightsabers.LightsaberIO; import frc.robot.subsystems.lightsabers.LightsaberReal; @@ -38,6 +41,7 @@ public class RobotContainer { /* Subsystems */ private Swerve s_Swerve; private Lightsaber s_Lightsabers; + private LEDs leds = new LEDs(Constants.LEDConstants.LED_COUNT, Constants.LEDConstants.PWM_PORT); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -60,6 +64,7 @@ public RobotContainer(RobotRunType runtimeType) { // Configure the button bindings s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop)); + leds.setDefaultCommand(new MovingColorLEDs(leds, Color.kRed, 4, false)); configureButtonBindings(); } @@ -71,6 +76,7 @@ public RobotContainer(RobotRunType runtimeType) { */ private void configureButtonBindings() { driver.a().whileTrue(s_Lightsabers.turnLightsabers(.2)); + driver.b().whileTrue(new MovingColorLEDs(leds, Color.kBlue, 4, false)); } /** diff --git a/src/main/java/frc/robot/commands/FlashingLEDColor.java b/src/main/java/frc/robot/commands/FlashingLEDColor.java new file mode 100644 index 0000000..19532e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/FlashingLEDColor.java @@ -0,0 +1,65 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.LEDs; + +/** Command to flash LEDs on and off once per second. */ +public class FlashingLEDColor extends Command { + private LEDs leds; + private int ledLength; + private int flashingDelay = 0; + private Color color; + private Color altColor; + + /** + * Command to flash the LED strip between 2 colors + * + * @param leds LED Subsystem + * @param color The first color + * @param altColor The second color + */ + public FlashingLEDColor(LEDs leds, Color color, Color altColor) { + this.leds = leds; + this.color = color; + this.altColor = altColor; + ledLength = leds.getLength(); + addRequirements(leds); + } + + /** + * Command to flash the LED strip between a color and black + * + * @param leds LED Subsystem + * @param color The color + */ + public FlashingLEDColor(LEDs leds, Color color) { + this(leds, color, Color.kBlack); + } + + @Override + public void execute() { + if (flashingDelay < 10) { + for (var i = 0; i < ledLength; i++) { + leds.setColor(i, color); + } + } else { + for (var i = 0; i < ledLength; i++) { + leds.setColor(i, altColor); + } + } + leds.setData(); + flashingDelay++; + flashingDelay %= 20; + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/MovingColorLEDs.java b/src/main/java/frc/robot/commands/MovingColorLEDs.java new file mode 100644 index 0000000..15396e4 --- /dev/null +++ b/src/main/java/frc/robot/commands/MovingColorLEDs.java @@ -0,0 +1,67 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.LEDs; + +/** + * Command to move LEDs back and forth like a Cylon eye + */ +public class MovingColorLEDs extends Command { + private final LEDs leds; + private int ledLength; + private Color color; + private int count; + private boolean inverted = false; + private int movingColorDelay = 0; + private int movingLED = 0; + private boolean movingDirection = true; + + /** + * Command to move LEDs back and forth like a Cylon eye + * + * @param leds LED subsystem + * @param color Color to which to set the LEDs + * @param count The number of LEDs in the moving dot. Best is an odd number. + * @param inverted Whether to invert the Color that moves. + */ + public MovingColorLEDs(LEDs leds, Color color, int count, boolean inverted) { + this.leds = leds; + this.color = color; + this.count = count + 2; + this.inverted = inverted; + ledLength = leds.getLength(); + addRequirements(leds); + } + + @Override + public void execute() { + Color theColor = inverted ? Color.kBlack : color; + Color secondColor = inverted ? color : Color.kBlack; + if (movingColorDelay == 0) { + for (var i = 0; i < ledLength; i++) { + if (Math.abs(i - movingLED) < count) { + leds.setColor(i, theColor); + } else { + leds.setColor(i, secondColor); + } + } + if (movingDirection) { + movingLED++; + } else { + movingLED--; + } + if (movingLED >= ledLength - 1 || movingLED <= 0) { + movingDirection = !movingDirection; + } + leds.setData(); + } + movingColorDelay += 1; + movingColorDelay %= 1; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} 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..72a0383 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -0,0 +1,110 @@ +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.SubsystemBase; + +/** + * This is the class header for the LEDs Subsystem + */ +public class LEDs extends SubsystemBase { + private AddressableLEDBuffer controLedBuffer; + @SuppressWarnings("IOCheck") + private AddressableLED addressableLED; + + /** + * constructs a LED Subsystem + * + * @param length length of the addressable LEDS + * @param port port ID for PWM + */ + public LEDs(int length, int port) { + controLedBuffer = new AddressableLEDBuffer(length); + addressableLED = new AddressableLED(port); + + addressableLED.setLength(controLedBuffer.getLength()); + addressableLED.setData(controLedBuffer); + addressableLED.start(); + } + + /** + * Get LED strip length + * + * @return number of LEDs + */ + public int getLength() { + return controLedBuffer.getLength(); + } + + /** + * Set individual LED color via HSV + * + * @param index the LED index to set + * @param h the h value [0-180) + * @param s the s value [0-255] + * @param v the v value [0-255] + */ + public void setHSV(int index, int h, int s, int v) { + controLedBuffer.setHSV(index, h, s, v); + } + + /** + * Sets the LED output data. + */ + public void setData() { + addressableLED.setData(controLedBuffer); + } + + /** + * Set individual LED color via RGB + * + * @param index the LED index to set + * @param r the r value [0-255] + * @param g the g value [0-255] + * @param b the b value [0-255] + */ + public void setRGB(int index, int r, int g, int b) { + controLedBuffer.setRGB(index, r, g, b); + } + + /** + * Sets RGB Color of the entire LED strip + * + * @param r - [0 - 255] + * @param g - [0 - 255] + * @param b - [0 - 255] + */ + public void setRGB(int r, int g, int b) { + for (var i = 0; i < getLength(); i++) { + setRGB(i, r, g, b); + } + setData(); + } + + /** + * Set individual LED color via Color + * + * @param index the LED index to set + * @param color The color of the LED + */ + public void setColor(int index, Color color) { + controLedBuffer.setLED(index, color); + } + + /** + * Sets the Color of the entire LED strip + * + * @param color color set for the LEDs + */ + public void setColor(Color color) { + for (var i = 0; i < getLength(); i++) { + setColor(i, color); + } + setData(); + } + + public Color getColor(int index) { + return controLedBuffer.getLED(index); + } +}