diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6b0756fb..388140e4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -94,6 +94,11 @@ private static RobotType determineRobotType() { return null; } + /** + * Set this to true to completely disable usage of the AddressableLED subsystem, no matter what + */ + public static boolean MASTER_LED_DISABLE = false; + private static final Alert wrongRobotTypeAlertReal = new Alert( String.format( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc6c589d..6b48023d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.commands.ManualAlignCommands; import frc.robot.commands.controllers.JoystickInputController; import frc.robot.commands.controllers.SpeedLevelController; +import frc.robot.subsystems.addressableled.AddressableLEDSubsystem; import frc.robot.subsystems.dashboard.DriverDashboard; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; @@ -79,7 +80,6 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Subsystems private final Drive drive; private final AprilTagVision vision; @@ -92,6 +92,8 @@ public class RobotContainer { private final Hang hang; + private final AddressableLEDSubsystem lights; + // Controller private final CommandXboxController driverController = new CommandXboxController(0); private final CommandXboxController operatorController = new CommandXboxController(1); @@ -126,6 +128,8 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, IO devices, and commands. */ public RobotContainer() { + final boolean lightsAreFake; + switch (Constants.getRobot()) { case COMP_BOT_2025: // Real robot (Competition bot with mechanisms), instantiate hardware IO implementations @@ -148,6 +152,8 @@ public RobotContainer() { hang = new Hang(new HangIOHardware(HangConstants.HANG_CONFIG)); coralWrist = new Wrist(new WristIOHardware(WristConstants.WRIST_CONFIG)); coralIntake = new Intake(new IntakeIOHardware(IntakeConstants.CORAL_INTAKE_CONFIG)); + + lightsAreFake = false; break; case WOOD_BOT_TWO_2025: @@ -164,6 +170,8 @@ public RobotContainer() { hang = new Hang(new HangIO() {}); coralWrist = new Wrist(new WristIO() {}); coralIntake = new Intake(new IntakeIO() {}); + + lightsAreFake = false; break; case T_SHIRT_CANNON_CHASSIS: @@ -180,6 +188,8 @@ public RobotContainer() { elevator = new Elevator(new ElevatorIO() {}); coralWrist = new Wrist(new WristIO() {}); coralIntake = new Intake(new IntakeIO() {}); + + lightsAreFake = true; break; case CRESCENDO_CHASSIS_2024: @@ -199,6 +209,7 @@ public RobotContainer() { coralIntake = new Intake(new IntakeIO() {}); + lightsAreFake = true; break; case SIM_BOT: @@ -221,6 +232,8 @@ public RobotContainer() { elevator = new Elevator(new ElevatorIOSim()); coralWrist = new Wrist(new WristIOSim(WristConstants.WRIST_CONFIG)); coralIntake = new Intake(new IntakeIOSim()); + + lightsAreFake = true; break; default: @@ -238,11 +251,14 @@ public RobotContainer() { coralWrist = new Wrist(new WristIO() {}); coralIntake = new Intake(new IntakeIO() {}); + lightsAreFake = true; break; } + lights = new AddressableLEDSubsystem(lightsAreFake); + // Superstructure - superstructure = new Superstructure(elevator, coralWrist, coralIntake); + superstructure = new Superstructure(elevator, coralWrist, coralIntake, lights); // Vision setup // vision.setLastRobotPoseSupplier(drive::getRobotPose); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index aba8b36f..1369b94b 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -119,7 +119,6 @@ public static Command joystickDriveWithSlowdown( DoubleSupplier omegaSupplier, DoubleSupplier elevatorHeightSupplier, BooleanSupplier useFieldRelativeSupplier) { - Runnable drive = () -> { Translation2d translation = translationSupplier.get(); diff --git a/src/main/java/frc/robot/commands/SetAddressableLEDPattern.java b/src/main/java/frc/robot/commands/SetAddressableLEDPattern.java new file mode 100644 index 00000000..7a966101 --- /dev/null +++ b/src/main/java/frc/robot/commands/SetAddressableLEDPattern.java @@ -0,0 +1,57 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.addressableled.AddressableLEDSubsystem; + +public class SetAddressableLEDPattern extends Command { + private final AddressableLEDSubsystem ledSystem; + + /** + * @apiNote If this is empty, that means that this command is targeting the whole strip + */ + private final int[] sections; + + private final LEDPattern pattern; + + /** + * @param led Addressable LED subsystem to use + * @param pattern Pattern to apply when command run + * @param section Section of LED strip to apply pattern to (index into + * AddressableLEDConstants.SECTIONS) + */ + public SetAddressableLEDPattern( + AddressableLEDSubsystem ledSystem, LEDPattern pattern, int... sections) { + this.sections = sections; + this.pattern = pattern; + this.ledSystem = ledSystem; + addRequirements(ledSystem); + } + + /** + * @param led Addressable LED subsystem to use + * @param pattern Pattern to apply when command run + */ + public SetAddressableLEDPattern(AddressableLEDSubsystem ledSystem, LEDPattern pattern) { + sections = new int[0]; + this.pattern = pattern; + this.ledSystem = ledSystem; + addRequirements(ledSystem); + } + + @Override + public void execute() { + if (sections.length <= 0) { + ledSystem.applyPattern(pattern); + } else { + for (int i = 0; i < sections.length; i++) { + ledSystem.applySectionedPattern(pattern, sections[i]); + } + } + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/SetBlinkinLEDPattern.java b/src/main/java/frc/robot/commands/SetBlinkinLEDPattern.java new file mode 100644 index 00000000..d563411a --- /dev/null +++ b/src/main/java/frc/robot/commands/SetBlinkinLEDPattern.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.blinkinled.BlinkinLEDSubsystem; + +public class SetBlinkinLEDPattern extends Command { + private BlinkinLEDSubsystem ledSystem; + private double pattern; + + /** + * @apiNote If this is -1, that means that this command is targeting the whole strip + */ + private int section; + + private boolean invalid; + + public SetBlinkinLEDPattern(BlinkinLEDSubsystem ledSystem, int section, double pattern) { + invalid = (section < -1 || section >= ledSystem.stripCount); + this.section = section; + this.ledSystem = ledSystem; + this.pattern = pattern; + } + + public SetBlinkinLEDPattern(BlinkinLEDSubsystem led, double pattern) { + this(led, -1, pattern); + } + + @Override + public void initialize() { + if (invalid) return; + if (section == -1) { + ledSystem.applyPatternToAll(pattern); + } else { + ledSystem.applyPatternTo(section, pattern); + } + } + + // Since executing the command is a one-time thing, we always report being done instantly + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/SetLightPattern.java b/src/main/java/frc/robot/commands/SetLightPattern.java deleted file mode 100644 index dd713fac..00000000 --- a/src/main/java/frc/robot/commands/SetLightPattern.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.led.LEDSubsystem; - -public class SetLightPattern extends Command { - private LEDSubsystem ledSystem; - private double pattern; - - // Set this to -1 to have the command apply to all strips - private int stripID; - - public SetLightPattern(LEDSubsystem led, int strip, double pattern) { - if (strip < -1) - throw new IllegalArgumentException( - "LED configuration commands may ONLY use -1 (all strips) or higher for strip IDs!"); - ledSystem = led; - this.pattern = pattern; - } - - public SetLightPattern(LEDSubsystem led, double pattern) { - this(led, -1, pattern); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - if (stripID == -1) { - ledSystem.applyPatternToAll(pattern); - } else { - ledSystem.applyPatternTo(stripID, pattern); - } - } - - // Since executing the command is a one-time thing, we always report being done instantly - - @Override - public boolean isFinished() { - return true; - } - - @Override - public void end(boolean interrupted) {} -} diff --git a/src/main/java/frc/robot/commands/test/AddressableLEDTestCommand.java b/src/main/java/frc/robot/commands/test/AddressableLEDTestCommand.java new file mode 100644 index 00000000..69b049a6 --- /dev/null +++ b/src/main/java/frc/robot/commands/test/AddressableLEDTestCommand.java @@ -0,0 +1,58 @@ +package frc.robot.commands.test; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.addressableled.AddressableLEDConstants; +import frc.robot.subsystems.addressableled.AddressableLEDSubsystem; + +public class AddressableLEDTestCommand extends Command { + private final AddressableLEDSubsystem ledSystem; + private LEDPattern currentPattern; + private int ticker = 0; + private int testCount = 0; + + public AddressableLEDTestCommand(AddressableLEDSubsystem ledSystem) { + this.ledSystem = ledSystem; + addRequirements(ledSystem); + } + + @Override + public void initialize() { + ticker = 0; + testCount = 0; + currentPattern = + LEDPattern.rainbow(255, 128) + .scrollAtAbsoluteSpeed(MetersPerSecond.of(1), AddressableLEDConstants.LED_DENSITY); + } + + @Override + public void execute() { + if (ticker < 500) { + ticker++; + } else { + ticker = 0; + ledSystem.applySectionedPattern(LEDPattern.kOff, testCount); + testCount++; + if (testCount == AddressableLEDConstants.SECTIONS.length) { + currentPattern = LEDPattern.solid(Color.kLimeGreen).breathe(Seconds.of(2)); + ledSystem.applyPattern(currentPattern); + } else { + ledSystem.applySectionedPattern(currentPattern, testCount); + } + } + } + + @Override + public boolean isFinished() { + return testCount >= (AddressableLEDConstants.SECTIONS.length + 1); + } + + @Override + public void end(boolean interrupted) { + ledSystem.applyPattern(LEDPattern.kOff); + } +} diff --git a/src/main/java/frc/robot/commands/test/LEDTestCommand.java b/src/main/java/frc/robot/commands/test/BlinkinLEDTestCommand.java similarity index 63% rename from src/main/java/frc/robot/commands/test/LEDTestCommand.java rename to src/main/java/frc/robot/commands/test/BlinkinLEDTestCommand.java index d5840d2e..4108253c 100644 --- a/src/main/java/frc/robot/commands/test/LEDTestCommand.java +++ b/src/main/java/frc/robot/commands/test/BlinkinLEDTestCommand.java @@ -1,11 +1,11 @@ package frc.robot.commands.test; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.led.LEDConstants; -import frc.robot.subsystems.led.LEDSubsystem; +import frc.robot.subsystems.blinkinled.BlinkinLEDConstants; +import frc.robot.subsystems.blinkinled.BlinkinLEDSubsystem; -public class LEDTestCommand extends Command { - private LEDSubsystem ledSystem; +public class BlinkinLEDTestCommand extends Command { + private BlinkinLEDSubsystem ledSystem; private double currentPattern; @@ -17,14 +17,14 @@ public class LEDTestCommand extends Command { * @param time Time in robot updates to wait between changes (each update is 20ms, so 50 updates * is 1 second) */ - public LEDTestCommand(LEDSubsystem led, int time) { + public BlinkinLEDTestCommand(BlinkinLEDSubsystem led, int time) { ledSystem = led; delay = time; } @Override public void initialize() { - currentPattern = LEDConstants.LEDPatterns.BLACK; + currentPattern = BlinkinLEDConstants.Patterns.BLACK; ticker = 0; } @@ -39,11 +39,11 @@ public void execute() { @Override public boolean isFinished() { - return currentPattern <= LEDConstants.LEDPatterns.HOT_PINK; + return currentPattern <= BlinkinLEDConstants.Patterns.HOT_PINK; } @Override public void end(boolean interrupted) { - currentPattern = LEDConstants.LEDPatterns.BLACK; + currentPattern = BlinkinLEDConstants.Patterns.BLACK; } } diff --git a/src/main/java/frc/robot/subsystems/addressableled/AddressableLEDConstants.java b/src/main/java/frc/robot/subsystems/addressableled/AddressableLEDConstants.java new file mode 100644 index 00000000..78fbc8fc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/addressableled/AddressableLEDConstants.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.addressableled; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.units.measure.Distance; + +public class AddressableLEDConstants { + /** + * This record is used to represent a range of LEDs that are grouped together and exhibit one pattern + * @param low The lowest index LED in the range + * @param high The highest index LED in the range + */ + public record Range(int low, int high) {} + + // TODO: Replace these with real values + public static final int LED_COUNT = 60; + public static final Distance LED_DENSITY = Meters.of(1.0 / 60.0); + public static final int LED_STRIP_PORT = 0; + + /** + * @apiNote The lower bound of the range represents the lowest index LED for this section, and the + * upper bound represents the highest index LED + *

Sections

+ * Assume front of robot refers to battery side + *
    + *
  1. Front + *
  2. Left + *
  3. Back + *
  4. Right + *
  5. Superstructure Left + *
  6. Superstructure Right + *
+ */ + public static final Range SECTIONS[] = { + new Range(0, 9), + new Range(10, 19), + new Range(20, 29), + new Range(30, 39), + new Range(40, 49), + new Range(50, 59) + }; +} diff --git a/src/main/java/frc/robot/subsystems/addressableled/AddressableLEDSubsystem.java b/src/main/java/frc/robot/subsystems/addressableled/AddressableLEDSubsystem.java new file mode 100644 index 00000000..193d088a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/addressableled/AddressableLEDSubsystem.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems.addressableled; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.AddressableLEDBufferView; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class AddressableLEDSubsystem extends SubsystemBase { + private final AddressableLED led; + private final AddressableLEDBuffer ledBuffer; + private AddressableLEDBufferView ledViews[]; + private LEDPattern[] currentPatterns; + private final boolean fake; + + /** + * @param isFake Set this to true to disable useful functions for robots that don't have LEDs + * attached + */ + public AddressableLEDSubsystem(boolean isFake) { + fake = isFake || Constants.MASTER_LED_DISABLE; + if (fake) { + led = null; + ledBuffer = null; + return; + } + + // Create strip and buffer + led = new AddressableLED(AddressableLEDConstants.LED_STRIP_PORT); + ledBuffer = new AddressableLEDBuffer(AddressableLEDConstants.LED_COUNT); + led.setLength(AddressableLEDConstants.LED_COUNT); + + // Create current pattern trackers + currentPatterns = new LEDPattern[AddressableLEDConstants.SECTIONS.length]; + + // Create section views + ledViews = new AddressableLEDBufferView[AddressableLEDConstants.SECTIONS.length]; + for (int i = 0; i < ledViews.length; i++) { + ledViews[i] = + new AddressableLEDBufferView( + ledBuffer, + AddressableLEDConstants.SECTIONS[i].low(), + AddressableLEDConstants.SECTIONS[i].high()); + } + } + + @Override + public void periodic() { + if (fake) return; + for (int i = 0; i < ledViews.length; i++) { + currentPatterns[i].applyTo(ledViews[i]); + } + led.setData(ledBuffer); + } + + /** + * Apply a pattern to a section of the LED strip + * + * @param pattern The pattern to apply + * @param section The section of the LED strip to apply. This is an index into + * AddressableLEDConstants.SECTIONS. + */ + public void applySectionedPattern(LEDPattern pattern, int section) { + if (fake) return; + if (section < 0 || section >= ledViews.length) return; + pattern.applyTo(ledViews[section]); + currentPatterns[section] = pattern; + } + + /** + * Apply a pattern to the entirety of the LED strip WARNING: This will overwrite pattern settings + * for all sections + * + * @param pattern The pattern to apply + */ + public void applyPattern(LEDPattern pattern) { + if (fake) return; + pattern.applyTo(ledBuffer); + for (int i = 0; i < currentPatterns.length; i++) { + currentPatterns[i] = pattern; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/led/LEDConstants.java b/src/main/java/frc/robot/subsystems/blinkinled/BlinkinLEDConstants.java similarity index 91% rename from src/main/java/frc/robot/subsystems/led/LEDConstants.java rename to src/main/java/frc/robot/subsystems/blinkinled/BlinkinLEDConstants.java index 0680007d..18fdd6af 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDConstants.java +++ b/src/main/java/frc/robot/subsystems/blinkinled/BlinkinLEDConstants.java @@ -1,11 +1,11 @@ -package frc.robot.subsystems.led; +package frc.robot.subsystems.blinkinled; -public class LEDConstants { +public class BlinkinLEDConstants { /** * @brief Patterns taken from * @link https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf */ - public static class LEDPatterns { + public static class Patterns { public static final double HOT_PINK = 0.57; public static final double DARK_RED = 0.59; public static final double RED = 0.61; diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/blinkinled/BlinkinLEDSubsystem.java similarity index 81% rename from src/main/java/frc/robot/subsystems/led/LEDSubsystem.java rename to src/main/java/frc/robot/subsystems/blinkinled/BlinkinLEDSubsystem.java index 30f75b04..94a0db97 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/blinkinled/BlinkinLEDSubsystem.java @@ -1,10 +1,10 @@ -package frc.robot.subsystems.led; +package frc.robot.subsystems.blinkinled; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.led.LEDConstants.LEDPatterns; +import frc.robot.subsystems.blinkinled.BlinkinLEDConstants.Patterns; -public class LEDSubsystem extends SubsystemBase { +public class BlinkinLEDSubsystem extends SubsystemBase { private class LEDStrip { public Spark pwm; public double pattern; @@ -16,12 +16,14 @@ public LEDStrip(Spark pwmController, double initialPattern) { } private LEDStrip[] strips; + public final int stripCount; - public LEDSubsystem(int... pwmPorts) { + public BlinkinLEDSubsystem(int... pwmPorts) { // Create all the strip objects + stripCount = pwmPorts.length; strips = new LEDStrip[pwmPorts.length]; for (int i = pwmPorts.length; i >= 0; i--) { - strips[i] = new LEDStrip(new Spark(pwmPorts[i]), LEDPatterns.BLACK); + strips[i] = new LEDStrip(new Spark(pwmPorts[i]), Patterns.BLACK); } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index ce2711a7..f973c071 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.SetAddressableLEDPattern; +import frc.robot.subsystems.addressableled.AddressableLEDSubsystem; import frc.robot.subsystems.superstructure.elevator.Elevator; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.subsystems.superstructure.wrist.Wrist; @@ -16,6 +18,7 @@ public class Superstructure extends VirtualSubsystem { private final Elevator elevator; private final Wrist coralWrist; private final Intake coralIntake; + private final AddressableLEDSubsystem lights; public static enum State { STOW, @@ -64,10 +67,12 @@ private State(boolean algae) { private final SuperstructureVisualizer goalVisualizer = new SuperstructureVisualizer("Goal", Color.kLime); - public Superstructure(Elevator elevator, Wrist coralWrist, Intake coralIntake) { + public Superstructure( + Elevator elevator, Wrist coralWrist, Intake coralIntake, AddressableLEDSubsystem lights) { this.elevator = elevator; this.coralWrist = coralWrist; this.coralIntake = coralIntake; + this.lights = lights; } public Command run(State goal) { @@ -125,9 +130,10 @@ public Command runWheels(State goal) { public static final double STOW_HEIGHT_HIGH = 0; public static final Rotation2d STOW_CORAL_ANGLE_HIGH = Rotation2d.fromDegrees(90); + // TODO: Integrate lightstrip commands into this stuff public Command prepareL1() { return Commands.parallel( - elevator.runPositionPrepare(L1_HEIGHT), coralWrist.runPositionPrepare(L1_CORAL_ANGLE)); + elevator.runPositionPrepare(L1_HEIGHT), coralWrist.runPositionPrepare(L1_CORAL_ANGLE)); } public Command prepareL2() { diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java index 4f94bf20..4f5b70e0 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java @@ -1,3 +1,12 @@ package frc.robot.subsystems.superstructure; -public class SuperstructureConstants {} +import edu.wpi.first.wpilibj.util.Color; + +public class SuperstructureConstants { + public static final Color STOW_LED_COLOR = Color.kSalmon; + public static final Color L1_COLOR = Color.kPurple; + public static final Color L2_COLOR = Color.kAqua; + public static final Color L3_COLOR = Color.kLime; + public static final Color L4_COLOR = Color.kYellow; + public static final Color INTAKE_COLOR = Color.kOrange; +}