diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 737fb591..f33044dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -135,7 +135,9 @@ public void robotPeriodic() { SmartDashboard.putString("DeploymentState", robotContainer.getDeployer().getDeploymentState().toString()); if (!Constants.TESTBED) { Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); - // Puts data on the elastic dashboard + 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 9d76a7c4..4a0aa744 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,7 +31,10 @@ import frc.robot.commands.intakeDeployment.ToggleDeployment; 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; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.DefaultAnglerControl; @@ -58,6 +61,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; @@ -113,6 +117,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; @@ -146,7 +151,7 @@ public RobotContainer() { hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo()); turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo()); - + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); @@ -161,7 +166,7 @@ public RobotContainer() { new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; - + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case REPLAY -> { // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); @@ -174,11 +179,13 @@ public RobotContainer() { turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo()); - // No GyroSubsystem in REPLAY for now + + // 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, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case SIM -> { @@ -195,11 +202,14 @@ 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) + + // 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(truster,drivebase), drivebase, truster) : null; - } + lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); + + } default -> { throw new RuntimeException("Did not specify Robot Mode"); @@ -498,7 +508,7 @@ public void putShuffleboardCommands() { new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); SmartDashboard.putData( "intakedeployer/Deployment State: STOPPED", - new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); + new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); } @@ -506,10 +516,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..4f39c8d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -0,0 +1,83 @@ +package frc.robot.commands.lightStrip; + +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; +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()) { + + if (shootingState.getShootState() != ShootState.STOPPED) { // Light strip only blinks if the shooting state is not stopped + 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/SetLedFromShootingState.java b/src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java new file mode 100644 index 00000000..cc3e2032 --- /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; + +// This code is unused. The light strip is fully controlled through SetLed. + +public class SetLedFromShootingState extends LoggableCommand{ + + private final LightStripSubsystem subsystem; + private ShootingState shootingState; + + public SetLedFromShootingState(LightStripSubsystem subsystem, ShootingState shootingState) { + this.subsystem = subsystem; + this.shootingState = shootingState; + addRequirements(subsystem); + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + switch (shootingState.getShootState()) { + case STOPPED: + subsystem.setPattern(BlinkinPattern.DARK_RED); + 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.GREEN); + break; + } + } + + @Override + public boolean isFinished() { + return true; + } + + @Override + public void end(boolean interrupted) { + + } + +} 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..11e02309 --- /dev/null +++ b/src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java @@ -0,0 +1,23 @@ +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; + +// 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) { + + super( + new SetShootingState(shootState, newState), + new SetLedFromShootingState(lightStrip, shootState) + ); + + } + +} 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/ConstantsReal2026.java b/src/main/java/frc/robot/constants/ConstantsReal2026.java index 45a17e47..3215fe9d 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/constants/enums/Trench.java b/src/main/java/frc/robot/constants/enums/Trench.java new file mode 100644 index 00000000..5040c621 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/Trench.java @@ -0,0 +1,32 @@ +package frc.robot.constants.enums; + +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), + 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 new file mode 100644 index 00000000..880cce68 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems; + +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{ + + public static final String LOGGING_NAME = "LightStripSubsystem"; + private final Spark io; + private BlinkinPattern pattern; + + 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) { + 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