From 0543e9e93bf314f917fdf4238ff7605f5e871376 Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Wed, 4 Mar 2026 18:49:20 -0500 Subject: [PATCH 1/5] Added a light strip subsystem --- src/main/java/frc/robot/RobotContainer.java | 16 +- .../lightStrip/SetLedFromShootingState.java | 57 +++++++ .../robot/constants/ConstantsReal2026.java | 1 + .../robot/constants/ConstantsTestbed2026.java | 2 + .../robot/subsystems/LightStripSubsystem.java | 27 +++ .../frc/robot/subsystems/RollerSubsystem.java | 4 +- .../frc/robot/subsystems/TiltSubsystem.java | 2 - .../java/frc/robot/utils/BlinkinPattern.java | 160 ++++++++++++++++++ 8 files changed, 260 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java create mode 100644 src/main/java/frc/robot/subsystems/LightStripSubsystem.java create mode 100644 src/main/java/frc/robot/utils/BlinkinPattern.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ebb851f3..b5f418e4 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.intakeDeployment.InitialRunDeployment; import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.lightStrip.SetLedFromShootingState; import frc.robot.commands.parallels.RunHopperAndFeeder; import frc.robot.commands.sequences.IntakeDownSequence; import frc.robot.commands.sequences.IntakeUpSequence; @@ -55,6 +56,7 @@ import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.GyroSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LightStripSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ClimberSubsystem; @@ -104,6 +106,7 @@ public class RobotContainer { private final ControllerSubsystem controllerSubsystem; private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; + private final LightStripSubsystem lightStripSubsystem; private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; @@ -137,7 +140,7 @@ public RobotContainer() { hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo()); turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo()); - + lightStripSubsystem = new LightStripSubsystem(); climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); @@ -166,7 +169,8 @@ public RobotContainer() { turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo()); - // No GyroSubsystem in REPLAY for now + lightStripSubsystem = new LightStripSubsystem(); + // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null; @@ -188,7 +192,8 @@ public RobotContainer() { shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); intakeDeployer = new IntakeDeployerSubsystem( IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now - // create the drive subsystem with null gyro (use default json) + lightStripSubsystem = new LightStripSubsystem(); + // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null; @@ -490,7 +495,10 @@ public void putShuffleboardCommands() { new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); SmartDashboard.putData( "intakedeployer/Deployment State: STOPPED", - new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); + new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); + SmartDashboard.putData( + "light", + new SetLedFromShootingState(lightStripSubsystem, shootState)); } diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java b/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java new file mode 100644 index 00000000..23b716c0 --- /dev/null +++ b/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java @@ -0,0 +1,57 @@ +package frc.robot.commands.lightStrip; + +import frc.robot.constants.enums.ShootingState; +import frc.robot.subsystems.LightStripSubsystem; +import frc.robot.utils.BlinkinPattern; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class SetLedFromShootingState extends LoggableCommand{ + + private final LightStripSubsystem subsystem; + private ShootingState shootingState; + // private final Timer timer; + + public SetLedFromShootingState(LightStripSubsystem subsystem, ShootingState shootingState) { + this.subsystem = subsystem; + this.shootingState = shootingState; + //timer = new Timer(); + } + + @Override + public void initialize() { + //timer.restart(); + } + + @Override + public void execute() { + switch (shootingState.getShootState()) { + case STOPPED: + subsystem.setPattern(BlinkinPattern.COLOR_WAVES_LAVA_PALETTE); + break; + case FIXED: + subsystem.setPattern(BlinkinPattern.COLOR_WAVES_OCEAN_PALETTE); + break; + case FIXED_2: + subsystem.setPattern(BlinkinPattern.RED_ORANGE); + break; + case SHOOTING_HUB: + subsystem.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE); + break; + case SHUTTLING: + subsystem.setPattern(BlinkinPattern.COLOR_WAVES_FOREST_PALETTE); + break; + } + } + + @Override + public boolean isFinished() { + return true; + //return timer.hasElapsed(3); + } + + @Override + public void end(boolean interrupted) { + + } + +} diff --git a/src/main/java/frc/robot/constants/ConstantsReal2026.java b/src/main/java/frc/robot/constants/ConstantsReal2026.java index d7b6f370..59369731 100644 --- a/src/main/java/frc/robot/constants/ConstantsReal2026.java +++ b/src/main/java/frc/robot/constants/ConstantsReal2026.java @@ -24,6 +24,7 @@ public class ConstantsReal2026 extends GameConstants { public static final double INITIAL_ROBOT_HEIGHT = 0; public static final int INTAKE_DIGITAL_INPUT_CHANNEL = 0; + public static final int LIGHT_STRIP_CHANNEL = 1; // Example ID, may change later public static final double GYRO_DIAGS_ANGLE = 30; diff --git a/src/main/java/frc/robot/constants/ConstantsTestbed2026.java b/src/main/java/frc/robot/constants/ConstantsTestbed2026.java index 95e26fa8..f98850d3 100644 --- a/src/main/java/frc/robot/constants/ConstantsTestbed2026.java +++ b/src/main/java/frc/robot/constants/ConstantsTestbed2026.java @@ -27,5 +27,7 @@ public class ConstantsTestbed2026 extends ConstantsPushbot2026 { // public static final int SHOOTER_MOTOR_ID = 50; // public static final int SHOOTER_FOLLOWER_MOTOR_ID = 40; + // public static final int LIGHT_STRIP_CHANNEL = 0; + public static final boolean TESTBED = true; } diff --git a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java new file mode 100644 index 00000000..69dfa8f3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.utils.BlinkinPattern; + +public class LightStripSubsystem extends SubsystemBase{ + + public static final String LOGGING_NAME = "LightStripSubsystem"; + private final Spark io; + private BlinkinPattern pattern; + + public LightStripSubsystem() { + this.io = new Spark(Constants.LIGHT_STRIP_CHANNEL); + } + + public void setPattern(BlinkinPattern pattern) { + this.pattern = pattern; + io.set(this.pattern.getPwm()); + } + + public BlinkinPattern getPattern() { + return pattern; + } + +} diff --git a/src/main/java/frc/robot/subsystems/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/RollerSubsystem.java index 16aee79f..a74ad167 100644 --- a/src/main/java/frc/robot/subsystems/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RollerSubsystem.java @@ -19,9 +19,7 @@ import frc.robot.utils.logging.io.motor.SimSparkMaxIo; import frc.robot.utils.logging.io.motor.SparkMaxIo; import frc.robot.utils.simulation.MotorSimulator; -import frc.robot.utils.simulation.RobotVisualizer; - -// The Roller subsystem spins the wheel that releases the algae. +import frc.robot.utils.simulation.RobotVisualizer; public class RollerSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "RollerSubsystem"; diff --git a/src/main/java/frc/robot/subsystems/TiltSubsystem.java b/src/main/java/frc/robot/subsystems/TiltSubsystem.java index d9b0f00b..161837b5 100644 --- a/src/main/java/frc/robot/subsystems/TiltSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TiltSubsystem.java @@ -22,8 +22,6 @@ import frc.robot.utils.simulation.ArmSimulator; import frc.robot.utils.simulation.RobotVisualizer; -// The Tilt subsystem extends and retracts the Algae-Go-Bye-Bye mechanism. - public class TiltSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "TiltSubsystem"; private final SparkMaxIo io; diff --git a/src/main/java/frc/robot/utils/BlinkinPattern.java b/src/main/java/frc/robot/utils/BlinkinPattern.java new file mode 100644 index 00000000..5d6caf00 --- /dev/null +++ b/src/main/java/frc/robot/utils/BlinkinPattern.java @@ -0,0 +1,160 @@ +package frc.robot.utils; + +/** + * Adapted from Link + */ +public enum BlinkinPattern { + /* + * Fixed Palette Pattern + */ + RAINBOW_RAINBOW_PALETTE, + RAINBOW_PARTY_PALETTE, + RAINBOW_OCEAN_PALETTE, + RAINBOW_LAVA_PALETTE, + RAINBOW_FOREST_PALETTE, + RAINBOW_WITH_GLITTER, + CONFETTI, + SHOT_RED, + SHOT_BLUE, + SHOT_WHITE, + SINELON_RAINBOW_PALETTE, + SINELON_PARTY_PALETTE, + SINELON_OCEAN_PALETTE, + SINELON_LAVA_PALETTE, + SINELON_FOREST_PALETTE, + BEATS_PER_MINUTE_RAINBOW_PALETTE, + BEATS_PER_MINUTE_PARTY_PALETTE, + BEATS_PER_MINUTE_OCEAN_PALETTE, + BEATS_PER_MINUTE_LAVA_PALETTE, + BEATS_PER_MINUTE_FOREST_PALETTE, + FIRE_MEDIUM, + FIRE_LARGE, + TWINKLES_RAINBOW_PALETTE, + TWINKLES_PARTY_PALETTE, + TWINKLES_OCEAN_PALETTE, + TWINKLES_LAVA_PALETTE, + TWINKLES_FOREST_PALETTE, + COLOR_WAVES_RAINBOW_PALETTE, + COLOR_WAVES_PARTY_PALETTE, + COLOR_WAVES_OCEAN_PALETTE, + COLOR_WAVES_LAVA_PALETTE, + COLOR_WAVES_FOREST_PALETTE, + LARSON_SCANNER_RED, + LARSON_SCANNER_GRAY, + LIGHT_CHASE_RED, + LIGHT_CHASE_BLUE, + LIGHT_CHASE_GRAY, + HEARTBEAT_RED, + HEARTBEAT_BLUE, + HEARTBEAT_WHITE, + HEARTBEAT_GRAY, + BREATH_RED, + BREATH_BLUE, + BREATH_GRAY, + STROBE_RED, + STROBE_BLUE, + STROBE_GOLD, + STROBE_WHITE, + /* + * CP1: Color 1 Pattern + */ + CP1_END_TO_END_BLEND_TO_BLACK, + CP1_LARSON_SCANNER, + CP1_LIGHT_CHASE, + CP1_HEARTBEAT_SLOW, + CP1_HEARTBEAT_MEDIUM, + CP1_HEARTBEAT_FAST, + CP1_BREATH_SLOW, + CP1_BREATH_FAST, + CP1_SHOT, + CP1_STROBE, + /* + * CP2: Color 2 Pattern + */ + CP2_END_TO_END_BLEND_TO_BLACK, + CP2_LARSON_SCANNER, + CP2_LIGHT_CHASE, + CP2_HEARTBEAT_SLOW, + CP2_HEARTBEAT_MEDIUM, + CP2_HEARTBEAT_FAST, + CP2_BREATH_SLOW, + CP2_BREATH_FAST, + CP2_SHOT, + CP2_STROBE, + /* + * CP1_2: Color 1 and 2 Pattern + */ + CP1_2_SPARKLE_1_ON_2, + CP1_2_SPARKLE_2_ON_1, + CP1_2_COLOR_GRADIENT, + CP1_2_BEATS_PER_MINUTE, + CP1_2_END_TO_END_BLEND_1_TO_2, + CP1_2_END_TO_END_BLEND, + CP1_2_NO_BLENDING, + CP1_2_TWINKLES, + CP1_2_COLOR_WAVES, + CP1_2_SINELON, + /* + * Solid color + */ + HOT_PINK, + DARK_RED, + RED, + RED_ORANGE, + ORANGE, + GOLD, + YELLOW, + LAWN_GREEN, + LIME, + DARK_GREEN, + GREEN, + BLUE_GREEN, + AQUA, + SKY_BLUE, + DARK_BLUE, + BLUE, + BLUE_VIOLET, + VIOLET, + WHITE, + GRAY, + DARK_GRAY, + BLACK; + /* + * Values are for REV Expansion Hub. Note that the expansion hub will output a + * range of 500 to 2500 mS. The first pattern expects 1005mS, ergo the position + * for the first pattern is 0.2525. + */ + private static final double start = -0.99; + private static final double increment = 0.02; + + public double getPwm() { + return start + (increment * this.ordinal()); + } + + public BlinkinPattern next() { + if (this.ordinal() + 1 < values().length) { + return values()[this.ordinal() + 1]; + } + return values()[0]; + } + + public BlinkinPattern previous() { + if (this.ordinal() - 1 > 0) { + return values()[this.ordinal() - 1]; + } + return values()[values().length - 1]; + } + + /** + * @param pwm pwm single that represents pattern + * @return the corresponding pattern or BLACK if that pwm value is invalid + */ + public static BlinkinPattern of(double pwm) { + double pos = (pwm - start) / increment; + if (pos == (int) pos) { + return values()[(int) pos]; + } + return BLACK; + } +} \ No newline at end of file From 1d9a9e37e75455d3b337e88f7a32b416d28e35de Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Wed, 4 Mar 2026 19:31:25 -0500 Subject: [PATCH 2/5] Set light color through the controller --- src/main/java/frc/robot/RobotContainer.java | 11 +++++----- .../lightStrip/SetLedFromShootingState.java | 4 ++-- .../parallels/SetShootingStateAndLight.java | 21 +++++++++++++++++++ 3 files changed, 29 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9fe81f5..f22edc13 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.lightStrip.SetLedFromShootingState; import frc.robot.commands.parallels.RunHopperAndFeeder; +import frc.robot.commands.parallels.SetShootingStateAndLight; import frc.robot.commands.sequences.IntakeDownSequence; import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.autochooser.AutoChooser; @@ -289,11 +290,11 @@ private void configureBindings() { controller.b().onTrue(new IntakeDownSequence(intakeDeployer, intakeSubsystem)); controller.y().onTrue(new ClimberUp(climberSubsystem)); controller.x().onTrue(new ClimberDown(climberSubsystem)); - controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); - controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); - controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); - controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); - driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); + controller.povUp().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.FIXED)); + controller.povRight().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.FIXED_2)); + controller.povDown().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.SHOOTING_HUB)); + controller.povLeft().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.SHUTTLING)); + driveJoystick.trigger().whileTrue((new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.STOPPED))); if (controllerSubsystem != null) { steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem)); } diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java b/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java index 23b716c0..ce5a25b3 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java @@ -26,7 +26,7 @@ public void initialize() { public void execute() { switch (shootingState.getShootState()) { case STOPPED: - subsystem.setPattern(BlinkinPattern.COLOR_WAVES_LAVA_PALETTE); + subsystem.setPattern(BlinkinPattern.DARK_RED); break; case FIXED: subsystem.setPattern(BlinkinPattern.COLOR_WAVES_OCEAN_PALETTE); @@ -38,7 +38,7 @@ public void execute() { subsystem.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE); break; case SHUTTLING: - subsystem.setPattern(BlinkinPattern.COLOR_WAVES_FOREST_PALETTE); + subsystem.setPattern(BlinkinPattern.GREEN); break; } } diff --git a/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java b/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java new file mode 100644 index 00000000..92612221 --- /dev/null +++ b/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java @@ -0,0 +1,21 @@ +package frc.robot.commands.parallels; + +import frc.robot.commands.lightStrip.SetLedFromShootingState; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.LightStripSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; + +public class SetShootingStateAndLight extends LoggableParallelCommandGroup{ + + public SetShootingStateAndLight(ShootingState shootState, LightStripSubsystem lightStrip, ShootState newState) { + + super( + new SetShootingState(shootState, newState), + new SetLedFromShootingState(lightStrip, shootState) + ); + + } + +} From 3bf09e3031a3270e1e3fa0c087b5efb8ce4d89fa Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Thu, 5 Mar 2026 18:16:19 -0500 Subject: [PATCH 3/5] Started adding trench alert functionality --- .../lightStrip/SetLedAsTrenchAlert.java | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java b/src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java new file mode 100644 index 00000000..95e90a8e --- /dev/null +++ b/src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java @@ -0,0 +1,44 @@ +package frc.robot.commands.lightStrip; + +import frc.robot.subsystems.LightStripSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class SetLedAsTrenchAlert extends LoggableCommand{ + + private final LightStripSubsystem lightStrip; + private final SwerveSubsystem drivebase; + private double robotX; + private double robotY; + + public SetLedAsTrenchAlert(LightStripSubsystem lightStrip, SwerveSubsystem drivebase) { + + this.lightStrip = lightStrip; + this.drivebase = drivebase; + + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + + robotX = drivebase.getPose().getX(); + robotY = drivebase.getPose().getY(); + + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + + } + +} From 9047d22d906c4873e19ccfebe50e0eee4ce41d18 Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Sun, 8 Mar 2026 15:34:58 -0400 Subject: [PATCH 4/5] Light changes based on shooting state and trench --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 31 +++---- .../frc/robot/commands/drive/FakeVision.java | 9 ++- .../frc/robot/commands/lightStrip/SetLed.java | 80 +++++++++++++++++++ .../lightStrip/SetLedAsTrenchAlert.java | 44 ---------- .../lightStrip/SetLedFromShootingState.java | 8 +- .../parallels/SetShootingStateAndLight.java | 2 + .../robot/constants/ConstantsPushbot2026.java | 1 + .../frc/robot/constants/enums/Trench.java | 30 +++++++ .../robot/subsystems/LightStripSubsystem.java | 6 +- 10 files changed, 148 insertions(+), 65 deletions(-) create mode 100644 src/main/java/frc/robot/commands/lightStrip/SetLed.java delete mode 100644 src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java create mode 100644 src/main/java/frc/robot/constants/enums/Trench.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..9848d76d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -122,6 +122,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if(!Constants.TESTBED){ Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); + SmartDashboard.putNumber("Robot X", robotContainer.getDriveBase().getPose().getX()); + SmartDashboard.putNumber("Robot Y", robotContainer.getDriveBase().getPose().getY()); // Puts data on the elastic dashboard SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f22edc13..6920026f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.commands.intakeDeployment.InitialRunDeployment; import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.lightStrip.SetLed; import frc.robot.commands.lightStrip.SetLedFromShootingState; import frc.robot.commands.parallels.RunHopperAndFeeder; import frc.robot.commands.parallels.SetShootingStateAndLight; @@ -142,7 +143,7 @@ public RobotContainer() { hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo()); turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo()); - lightStripSubsystem = new LightStripSubsystem(); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); @@ -157,7 +158,7 @@ public RobotContainer() { new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; - + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case REPLAY -> { // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); @@ -170,12 +171,13 @@ public RobotContainer() { turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo()); - lightStripSubsystem = new LightStripSubsystem(); + // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case SIM -> { @@ -192,12 +194,14 @@ public RobotContainer() { shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); intakeDeployer = new IntakeDeployerSubsystem( IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now - lightStripSubsystem = new LightStripSubsystem(); + // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null; - } + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); + + } default -> { throw new RuntimeException("Did not specify Robot Mode"); @@ -290,11 +294,11 @@ private void configureBindings() { controller.b().onTrue(new IntakeDownSequence(intakeDeployer, intakeSubsystem)); controller.y().onTrue(new ClimberUp(climberSubsystem)); controller.x().onTrue(new ClimberDown(climberSubsystem)); - controller.povUp().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.FIXED)); - controller.povRight().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.FIXED_2)); - controller.povDown().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.SHOOTING_HUB)); - controller.povLeft().onTrue(new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.SHUTTLING)); - driveJoystick.trigger().whileTrue((new SetShootingStateAndLight(shootState, lightStripSubsystem, ShootState.STOPPED))); + controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); + controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); + controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); + controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); + driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); if (controllerSubsystem != null) { steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem)); } @@ -493,9 +497,6 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "intakedeployer/Deployment State: STOPPED", new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); - SmartDashboard.putData( - "light", - new SetLedFromShootingState(lightStripSubsystem, shootState)); } @@ -503,10 +504,12 @@ public void putShuffleboardCommands() { if (!Constants.TESTBED) { Command driveDirectionTime = new DriveDirectionTime(drivebase, 0.1, 0.1, true, 1); SmartDashboard.putData("Drive Command", driveDirectionTime); - SmartDashboard.putData("Fake vision", new FakeVision(drivebase)); + SmartDashboard.putData("Fake vision near trench", new FakeVision(drivebase, 4, 1)); + SmartDashboard.putData("Fake vision away from trench", new FakeVision(drivebase, 1, 1)); SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); SmartDashboard.putData("AddGarbageReading", new AddGarbageReading(apriltagSubsystem)); SmartDashboard.putData("AddTunedApriltagReading", new AddTunableApriltagReading(apriltagSubsystem)); + } } diff --git a/src/main/java/frc/robot/commands/drive/FakeVision.java b/src/main/java/frc/robot/commands/drive/FakeVision.java index 4047aacf..3fc07bde 100644 --- a/src/main/java/frc/robot/commands/drive/FakeVision.java +++ b/src/main/java/frc/robot/commands/drive/FakeVision.java @@ -14,15 +14,20 @@ public class FakeVision extends LoggableCommand { /** Creates a new FakeVision. */ private final SwerveSubsystem drivebase; - public FakeVision(SwerveSubsystem drivebase) { + private double x; + private double y; + + public FakeVision(SwerveSubsystem drivebase, double x, double y) { // Use addRequirements() here to declare subsystem dependencies. this.drivebase = drivebase; + this.x = x; + this.y = y; } // Called when the command is initially scheduled. @Override public void initialize() { - drivebase.addVisionMeasurement(new Pose2d(new Translation2d(-10, 2), new Rotation2d())); + drivebase.addVisionMeasurement(new Pose2d(new Translation2d(x, y), new Rotation2d())); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java new file mode 100644 index 00000000..d2d3969f --- /dev/null +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -0,0 +1,80 @@ +package frc.robot.commands.lightStrip; + +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.Trench; +import frc.robot.subsystems.LightStripSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.BlinkinPattern; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class SetLed extends LoggableCommand{ + + private final LightStripSubsystem lightStrip; + private final SwerveSubsystem drivebase; + private ShootingState shootingState; + private double x; + private double y; + + public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState) { + + this.lightStrip = lightStrip; + this.drivebase = drivebase; + this.shootingState = shootingState; + addRequirements(lightStrip); + + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + + x = drivebase.getPose().getX(); + y = drivebase.getPose().getY(); + + // If statement checks if the robot is near the trench + if (x > Trench.RED_BOTTOM_LOWER.getX() && x < Trench.RED_BOTTOM_HIGHER.getX() && y > Trench.RED_BOTTOM_LOWER.getY() && y < Trench.RED_BOTTOM_HIGHER.getY() || + x > Trench.RED_TOP_LOWER.getX() && x < Trench.RED_TOP_HIGHER.getX() && y > Trench.RED_TOP_LOWER.getY() && y < Trench.RED_TOP_HIGHER.getY() || + x > Trench.BLUE_BOTTOM_LOWER.getX() && x < Trench.BLUE_BOTTOM_HIGHER.getX() && y > Trench.BLUE_BOTTOM_LOWER.getY() && y < Trench.BLUE_BOTTOM_HIGHER.getY() || + x > Trench.BLUE_TOP_LOWER.getX() && x < Trench.BLUE_TOP_HIGHER.getX() && y > Trench.BLUE_TOP_LOWER.getY() && y < Trench.BLUE_TOP_HIGHER.getY()) { + + lightStrip.setPattern(BlinkinPattern.STROBE_RED); + + } else { + + switch (shootingState.getShootState()) { + case STOPPED: + lightStrip.setPattern(BlinkinPattern.WHITE); + break; + case FIXED: + lightStrip.setPattern(BlinkinPattern.COLOR_WAVES_OCEAN_PALETTE); + break; + case FIXED_2: + lightStrip.setPattern(BlinkinPattern.RED_ORANGE); + break; + case SHOOTING_HUB: + lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE); + break; + case SHUTTLING: + lightStrip.setPattern(BlinkinPattern.GREEN); + break; + } + + } + + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + + } + +} diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java b/src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java deleted file mode 100644 index 95e90a8e..00000000 --- a/src/main/java/frc/robot/commands/lightStrip/SetLedAsTrenchAlert.java +++ /dev/null @@ -1,44 +0,0 @@ -package frc.robot.commands.lightStrip; - -import frc.robot.subsystems.LightStripSubsystem; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import frc.robot.utils.logging.commands.LoggableCommand; - -public class SetLedAsTrenchAlert extends LoggableCommand{ - - private final LightStripSubsystem lightStrip; - private final SwerveSubsystem drivebase; - private double robotX; - private double robotY; - - public SetLedAsTrenchAlert(LightStripSubsystem lightStrip, SwerveSubsystem drivebase) { - - this.lightStrip = lightStrip; - this.drivebase = drivebase; - - } - - @Override - public void initialize() { - - } - - @Override - public void execute() { - - robotX = drivebase.getPose().getX(); - robotY = drivebase.getPose().getY(); - - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - - } - -} diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java b/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java index ce5a25b3..cc3e2032 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java @@ -5,21 +5,22 @@ import frc.robot.utils.BlinkinPattern; import frc.robot.utils.logging.commands.LoggableCommand; +// This code is unused. The light strip is fully controlled through SetLed. + public class SetLedFromShootingState extends LoggableCommand{ private final LightStripSubsystem subsystem; private ShootingState shootingState; - // private final Timer timer; public SetLedFromShootingState(LightStripSubsystem subsystem, ShootingState shootingState) { this.subsystem = subsystem; this.shootingState = shootingState; - //timer = new Timer(); + addRequirements(subsystem); } @Override public void initialize() { - //timer.restart(); + } @Override @@ -46,7 +47,6 @@ public void execute() { @Override public boolean isFinished() { return true; - //return timer.hasElapsed(3); } @Override diff --git a/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java b/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java index 92612221..11e02309 100644 --- a/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java +++ b/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java @@ -7,6 +7,8 @@ import frc.robot.subsystems.LightStripSubsystem; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +// This code is unused. The light strip is fully controlled through SetLed. + public class SetShootingStateAndLight extends LoggableParallelCommandGroup{ public SetShootingStateAndLight(ShootingState shootState, LightStripSubsystem lightStrip, ShootState newState) { diff --git a/src/main/java/frc/robot/constants/ConstantsPushbot2026.java b/src/main/java/frc/robot/constants/ConstantsPushbot2026.java index 42ed7a3d..29de82a5 100644 --- a/src/main/java/frc/robot/constants/ConstantsPushbot2026.java +++ b/src/main/java/frc/robot/constants/ConstantsPushbot2026.java @@ -22,6 +22,7 @@ public class ConstantsPushbot2026 extends GameConstants { public static final double INITIAL_ROBOT_HEIGHT = 0; public static final int INTAKE_DIGITAL_INPUT_CHANNEL = 0; + public static final int LIGHT_STRIP_CHANNEL = 0; public static final double GYRO_DIAGS_ANGLE = 30; diff --git a/src/main/java/frc/robot/constants/enums/Trench.java b/src/main/java/frc/robot/constants/enums/Trench.java new file mode 100644 index 00000000..7b128387 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/Trench.java @@ -0,0 +1,30 @@ +package frc.robot.constants.enums; + +public enum Trench { + + RED_BOTTOM_LOWER(10,0), + RED_BOTTOM_HIGHER(14,1.5), + RED_TOP_LOWER(10,6.5), + RED_TOP_HIGHER(14,8), + BLUE_BOTTOM_LOWER(2.5,0), + BLUE_BOTTOM_HIGHER(6.5,1.5), + BLUE_TOP_LOWER(2.5,6.5), + BLUE_TOP_HIGHER(6.5,8); + + private double x; + private double y; + + private Trench(double x, double y) { + this.x = x; + this.y = y; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + +} diff --git a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java index 69dfa8f3..880cce68 100644 --- a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java @@ -2,7 +2,10 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.commands.lightStrip.SetLed; import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.BlinkinPattern; public class LightStripSubsystem extends SubsystemBase{ @@ -11,8 +14,9 @@ public class LightStripSubsystem extends SubsystemBase{ private final Spark io; private BlinkinPattern pattern; - public LightStripSubsystem() { + public LightStripSubsystem(SwerveSubsystem drivebase, ShootingState shootingState) { this.io = new Spark(Constants.LIGHT_STRIP_CHANNEL); + setDefaultCommand(new SetLed(this, drivebase, shootingState)); } public void setPattern(BlinkinPattern pattern) { From 4fc7a3c1597ec3c68b602c113f7a6e1692838803 Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Tue, 10 Mar 2026 20:30:42 -0400 Subject: [PATCH 5/5] LED does not blink if shooting state is stopped --- src/main/java/frc/robot/commands/lightStrip/SetLed.java | 5 ++++- src/main/java/frc/robot/constants/enums/Trench.java | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index d2d3969f..4f39c8d9 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -2,6 +2,7 @@ import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.Trench; +import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.LightStripSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.BlinkinPattern; @@ -41,7 +42,9 @@ public void execute() { x > Trench.BLUE_BOTTOM_LOWER.getX() && x < Trench.BLUE_BOTTOM_HIGHER.getX() && y > Trench.BLUE_BOTTOM_LOWER.getY() && y < Trench.BLUE_BOTTOM_HIGHER.getY() || x > Trench.BLUE_TOP_LOWER.getX() && x < Trench.BLUE_TOP_HIGHER.getX() && y > Trench.BLUE_TOP_LOWER.getY() && y < Trench.BLUE_TOP_HIGHER.getY()) { - lightStrip.setPattern(BlinkinPattern.STROBE_RED); + if (shootingState.getShootState() != ShootState.STOPPED) { // Light strip only blinks if the shooting state is not stopped + lightStrip.setPattern(BlinkinPattern.STROBE_RED); + } } else { diff --git a/src/main/java/frc/robot/constants/enums/Trench.java b/src/main/java/frc/robot/constants/enums/Trench.java index 7b128387..5040c621 100644 --- a/src/main/java/frc/robot/constants/enums/Trench.java +++ b/src/main/java/frc/robot/constants/enums/Trench.java @@ -2,6 +2,8 @@ public enum Trench { + // These are not exact trench boundaries. These trench zones + // include some space around the trench boundaries. RED_BOTTOM_LOWER(10,0), RED_BOTTOM_HIGHER(14,1.5), RED_TOP_LOWER(10,6.5),