diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index bf72a3e..04a91cd 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -90,9 +90,12 @@ private static class operatorBindings { public static final JoystickButton operatorControllerBack = new JoystickButton(operatorController, operatorBindings.BACK_BUTTON); // Trigger controls for coral claw + public static DoubleSupplier operatorRightTriggerSupplier= () -> { return operatorController.getRightTriggerAxis();}; public static final Trigger operatorRightTrigger = new Trigger(() -> operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD); + public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();}; + public static final Trigger operatorLeftTrigger = new Trigger(() -> operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab2e77c..4fe7a85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ public class RobotContainer { public Camera camera = new Camera(); public Drivetrain drivetrain = new Drivetrain(leftBackSwerve, rightBackSwerve, leftFrontSwerve, rightFrontSwerve, gyro); + public LEDStrip ledStrip = new LEDStrip(RobotMap.PWM.LED_STRIP_PORT, RobotMap.LED_STRIP_LENGTH); /** * The RobotContainer class is where the bulk of the robot should be declared. @@ -66,10 +67,17 @@ private void configureBindings() { // Elevator level controls // OPERATOR B : INTAKE OI.operatorControllerB.onTrue(new IntakeCommandGroup(elevator, wrist, coralClaw)); + OI.operatorControllerB.onTrue(Commands.runOnce(() -> { + ledStrip.indicateIntake(); + })); // OPERATOR RB : TROUGH(L1) OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.LOW)); + OI.operatorControllerRightBumper.onTrue(Commands.runOnce(() -> { + ledStrip.indicateScoring(); + })); // OPERATOR A : L2 (lowest pole) - OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.MED)); + OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.MED).alongWith( + Commands.runOnce(() -> ledStrip.indicateScoring()))); // OPERATOR X : L3 (2nd pole) OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.HIGH)); // OPERATOR Y : L4 (last pole) @@ -84,9 +92,9 @@ private void configureBindings() { OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175))); // OPERATOR RT : INTAKE - OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, 1)); + OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorRightTriggerSupplier, 1)); // OPERATOR LT : OUTTAKE - OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, 0)); + OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorLeftTriggerSupplier, 0)); // Manual control with right stick for testing in simulation // OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> @@ -94,7 +102,7 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return new ScoreTest(drivetrain, gyro, camera); + return new ScoreTest(drivetrain, gyro, camera, elevator, wrist, coralClaw); // return new DriveDistance(drivetrain, gyro, 7.0, 0.0, 0.0).andThen(new DriveDistance(drivetrain, gyro, 0.0, 3.0, 0.0)); // return new DriveWithHeadingCommand(drivetrain, gyro, OI.xboxLeftStickXSupplier, OI.xboxRightStickYSupplier, new Rotation2d(0.0)); // return new DriveMeters(drivetrain, 0.0, 0.0, 0.0); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index fcbcbc3..125e296 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -69,6 +69,11 @@ private class DIO { private static final int CORAL_LIMIT_SWITCH = 0; // Digital I/O port for coral limit switch } + // PWM port configuration + public class PWM { + public static final int LED_STRIP_PORT = 0; // PWM port for LED strip + } + // Pneumatic port configuration private class Pneumatics { private static final int WRIST_FORWARD_CHANNEL = 0; @@ -113,6 +118,9 @@ private class Pneumatics { // Coral claw limit switch public static final DigitalInput coralLimitSwitch = new DigitalInput(DIO.CORAL_LIMIT_SWITCH); + // LED strip + public static final int LED_STRIP_LENGTH = 60; // Number of LEDs in the strip + // Remove the subsystem instances from here // public static final Elevator elevator = new Elevator(); // public static final Wrist wrist = new Wrist(wristSolenoid); diff --git a/src/main/java/frc/robot/commands/ScoreCommandGroup.java b/src/main/java/frc/robot/commands/ScoreCommandGroup.java index a85bcb8..3db5edf 100644 --- a/src/main/java/frc/robot/commands/ScoreCommandGroup.java +++ b/src/main/java/frc/robot/commands/ScoreCommandGroup.java @@ -11,6 +11,11 @@ public class ScoreCommandGroup extends ParallelCommandGroup { public ScoreCommandGroup(Elevator elevator, Wrist wrist, CoralClaw claw, ElevatorLevel level) { + if(level == ElevatorLevel.LOW) { + addCommands(new InstantCommand(() -> claw.setMotorsOn(false, true))); + } else { + addCommands(new InstantCommand(() -> claw.setMotorsOn(true, true))); + } addCommands( new InstantCommand(() -> elevator.setTarget(level)), new InstantCommand(() -> wrist.extend()) diff --git a/src/main/java/frc/robot/commands/auto/DriveDistance.java b/src/main/java/frc/robot/commands/auto/DriveDistance.java index d347bd6..316b47c 100644 --- a/src/main/java/frc/robot/commands/auto/DriveDistance.java +++ b/src/main/java/frc/robot/commands/auto/DriveDistance.java @@ -21,21 +21,23 @@ public class DriveDistance extends Command { double angleDisplacement; double xyThreshold = 0.6; - double xyMaxSpeed = 0.65; + double xyMaxSpeed; double rotateMaxSpeed = 1.5; - public DriveDistance(Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) { + public DriveDistance(double xyMaxSpeed, Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) { this.drivetrain = drivetrain; this.gyro = gyro; this.xDistance = xDistance; this.yDistance = yDistance; this.theta = theta; + this.xyMaxSpeed = xyMaxSpeed; addRequirements(drivetrain); } @Override public void initialize() { + System.out.println("drivedistance initialized"); Pose2d start = drivetrain.getPose2d(); this.target = new Pose2d((start.getX() + xDistance), (start.getY() + yDistance), new Rotation2d(theta)); } @@ -84,7 +86,7 @@ public void execute() { @Override public void end(boolean interrupted) { - drivetrain.driveRobotRelative(0.0, 0.0, 0.0); + drivetrain.drive(0.0, 0.0, 0.0); } @Override diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index dd61bad..4b429b6 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -2,9 +2,17 @@ import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Gyro; import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Wrist; +import frc.robot.subsystems.CoralClaw; import frc.robot.commands.auto.DriveDistance; +import frc.robot.commands.IntakeCommandGroup; +import frc.robot.commands.ScoreCommandGroup; +import frc.robot.ElevatorLevel; + import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; /** * The ScoreTest command is a SequentialCommandGroup that performs a series of driving maneuvers @@ -37,14 +45,52 @@ public class ScoreTest extends SequentialCommandGroup { * @param gyro The gyro sensor used for orientation and heading. * @param camera The camera used for vision processing (currently commented out). */ - public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera) { - // ID7 from outer + public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, Wrist wrist, CoralClaw claw) { addCommands( new InstantCommand(() -> drivetrain.resetPosition()), - new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall - new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7 - new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command - new DriveTowardsThing(drivetrain, camera) + new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), + new DriveDistance(0.25, drivetrain, gyro, 0.0, 2.1, 1.0472), + new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + new WaitCommand(0.5), + new InstantCommand(() -> claw.setOutput(-0.4)), + new WaitCommand(1.0), + new InstantCommand(() -> claw.setOutput(0.0)) ); + + // SIDE 2 + // addCommands( + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), + // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.1, 5.23599), + // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + // new WaitCommand(500), + // new InstantCommand(() -> claw.setOutput(-0.4)), + // new WaitCommand(1000.0), + // new InstantCommand(() -> claw.setOutput(0.0)) + // ); + + + // new InstantCommand(() -> drivetrain.driveRobotRelative(0.25, 0.0, 0.0)) + + // SIDE 1 L1 + // addCommands( + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall + // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + // new WaitCommand(500.0), + // new InstantCommand(() -> claw.setOutput(-0.4)), + // new WaitCommand(1000.0), + // new InstantCommand(() -> claw.setOutput(0.0)) + // ); + + + // ID7 from outer + // addCommands( + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall + // new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7 + // new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command + // new DriveTowardsThing(drivetrain, camera) + // ); } } diff --git a/src/main/java/frc/robot/commands/instant/ClawCommand.java b/src/main/java/frc/robot/commands/instant/ClawCommand.java index 663489f..b3be853 100644 --- a/src/main/java/frc/robot/commands/instant/ClawCommand.java +++ b/src/main/java/frc/robot/commands/instant/ClawCommand.java @@ -3,17 +3,20 @@ import frc.robot.subsystems.CoralClaw; import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; public class ClawCommand extends Command { CoralClaw claw; double speed; int outin; + DoubleSupplier triggerSupplier; - public ClawCommand(CoralClaw claw, int outin) { + public ClawCommand(CoralClaw claw, DoubleSupplier triggerSupplier, int outin) { addRequirements(claw); this.claw = claw; this.speed = claw.INTAKE_SPEED; this.outin = outin; + this.triggerSupplier = triggerSupplier; } @Override @@ -22,13 +25,9 @@ public ClawCommand(CoralClaw claw, int outin) { @Override public void execute() { if(outin == 0) { // OUT - System.out.println("CLAWCOMMAND OUTTAKE"); - claw.outtake(); + claw.setOutput(-(triggerSupplier.getAsDouble() * 0.5 + 0.2)); } else { // INTAKE - System.out.println("CLAWCOMMAND INTAKE"); - claw.intake(); - // if(!claw.isCoralDetected()) { - // } + claw.setOutput(triggerSupplier.getAsDouble() * 0.5 + 0.2); } diff --git a/src/main/java/frc/robot/subsystems/CoralClaw.java b/src/main/java/frc/robot/subsystems/CoralClaw.java index 3faf09f..a850410 100644 --- a/src/main/java/frc/robot/subsystems/CoralClaw.java +++ b/src/main/java/frc/robot/subsystems/CoralClaw.java @@ -84,7 +84,8 @@ public class CoralClaw extends SubsystemBase { // Track previous coral state to detect transitions private boolean previousCoralDetected = false; - + + private boolean[] motorsOn = {true, true}; /** * Creates a new CoralClaw subsystem. * @@ -101,8 +102,8 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li // Configure the secondary motor to follow the primary motor with inversion SparkMaxConfig config = new SparkMaxConfig(); primaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - config.follow(primaryMotor, true); // true makes it follow with inversion secondaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // config.follow(primaryMotor, true); // true makes it follow with inversion // Initialize simulation-specific components if in simulation isSimulation = RobotBase.isSimulation(); @@ -118,8 +119,8 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li * Will run until a coral is detected or manually stopped */ public void intake() { - System.out.println("CLAW INTAKE"); - primaryMotor.set(INTAKE_SPEED); + if(motorsOn[0]) { primaryMotor.set(INTAKE_SPEED); } + if(motorsOn[1]) { secondaryMotor.set(INTAKE_SPEED); } // Secondary motor automatically follows with inversion // autoIntakeMode = true; SmartDashboard.putString("Coral Claw State", "Intaking (Auto)"); @@ -129,18 +130,26 @@ public void intake() { * Spins the wheels to outtake/eject a coral game piece */ public void outtake() { - System.out.println("CLAW OUTTAKE"); - primaryMotor.set(OUTTAKE_SPEED); + if(motorsOn[0]) { primaryMotor.set(OUTTAKE_SPEED); } + if(motorsOn[1]) { secondaryMotor.set(OUTTAKE_SPEED); } + // Secondary motor automatically follows with inversion // autoIntakeMode = false; SmartDashboard.putString("Coral Claw State", "Outtaking"); } + + public void setMotorsOn(boolean left, boolean right) { + motorsOn[0] = left; + motorsOn[1] = right; + } + /** * Stops the intake/outtake wheels */ public void stop() { primaryMotor.set(0); + secondaryMotor.set(0); // Secondary motor automatically follows with inversion // autoIntakeMode = false; SmartDashboard.putString("Coral Claw State", "Stopped"); @@ -192,9 +201,9 @@ public void setSimulatedCoralDetected(boolean detected) { * set claw output to @param out **/ public void setOutput(double out) { - System.out.println("claw set output; " + out); - primaryMotor.set(INTAKE_SPEED); - + if(motorsOn[0]) { primaryMotor.set(out); } + if(motorsOn[1]) { secondaryMotor.set(out); } + primaryMotor.set(out); } @Override diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 4ea7199..727d383 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -71,7 +71,6 @@ public Drivetrain(SwerveModule leftBackSwerve, } public void drive(double x, double y, double omega) { - System.out.println("TODO:REMOVETHIS DRIVE(" + x + ", " + y + ", " + omega + ")"); // x: x meters / second , y: y meters per second, omega: angular velocity // (radians per second) ChassisSpeeds chassisSpeed = ChassisSpeeds diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index cb026d6..52e28ea 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -153,7 +153,7 @@ public void setTarget(ElevatorLevel level) { // Tune these values to match the actual robot requirmenets to seed the coral switch (level) { case STATION: - groundHeight = Units.inchesToMeters(28.0); // all these measurements are from measuring the field components + groundHeight = Units.inchesToMeters(27.5); // all these measurements are from measuring the field components break; case LOW: groundHeight = Units.inchesToMeters(18.0); @@ -162,10 +162,10 @@ public void setTarget(ElevatorLevel level) { groundHeight = Units.inchesToMeters(29.75); break; case HIGH: - groundHeight = Units.inchesToMeters(42.5); + groundHeight = Units.inchesToMeters(42.25); break; case EXTRA_HIGH: - groundHeight = Units.inchesToMeters(64.75); + groundHeight = Units.inchesToMeters(63.75); break; default: groundHeight = 0.0; diff --git a/src/main/java/frc/robot/subsystems/LEDStrip.java b/src/main/java/frc/robot/subsystems/LEDStrip.java new file mode 100644 index 0000000..098f5ea --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDStrip.java @@ -0,0 +1,306 @@ +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; +import edu.wpi.first.wpilibj.Timer; + +/** + * Controls WS2812 addressable LED strips connected to the roboRIO PWM ports. + * Supports various patterns and animations to indicate robot state. + */ +public class LEDStrip extends SubsystemBase { + private final AddressableLED ledStrip; + private final AddressableLEDBuffer ledBuffer; + private final int length; + + // Animation variables + private int rainbowFirstPixelHue = 0; + private int animationCounter = 0; + private double lastAnimationTime = 0; + + // LED pattern states + private enum LEDPattern { + SOLID, + BREATHING, + RAINBOW, + CHASE, + ALTERNATING, + BLINKING + } + + private LEDPattern currentPattern = LEDPattern.SOLID; + private Color primaryColor = Color.kGreen; + private Color secondaryColor = Color.kDarkBlue; + private double animationSpeed = 0.05; // seconds between frames + + /** + * Creates a new LEDStrip subsystem. + * + * @param port PWM port the LED strip is connected to + * @param length Number of LEDs in the strip + */ + public LEDStrip(int port, int length) { + this.length = length; + + ledStrip = new AddressableLED(port); + ledBuffer = new AddressableLEDBuffer(length); + + // Set the data length + ledStrip.setLength(length); + + // Initialize all LEDs to off + for (var i = 0; i < length; i++) { + setLED(i, 0, 0, 0); + } + + // Send the initial data + ledStrip.setData(ledBuffer); + ledStrip.start(); + } + + /** + * Sets a single LED to a specified RGB color. + * + * @param index LED index + * @param r Red (0-255) + * @param g Green (0-255) + * @param b Blue (0-255) + */ + public void setLED(int index, int r, int g, int b) { + if (index >= 0 && index < length) { + ledBuffer.setRGB(index, r, g, b); + } + } + + /** + * Sets a single LED to a specified color. + * + * @param index LED index + * @param color Color to set + */ + public void setLED(int index, Color color) { + if (index >= 0 && index < length) { + ledBuffer.setLED(index, color); + } + } + + /** + * Sets the entire LED strip to a single color. + * + * @param color Color to set + */ + public void setSolidColor(Color color) { + currentPattern = LEDPattern.SOLID; + primaryColor = color; + + for (var i = 0; i < length; i++) { + ledBuffer.setLED(i, color); + } + ledStrip.setData(ledBuffer); + } + + /** + * Sets the LED strip to alternate between two colors. + * + * @param color1 First color + * @param color2 Second color + */ + public void setAlternatingColors(Color color1, Color color2) { + currentPattern = LEDPattern.ALTERNATING; + primaryColor = color1; + secondaryColor = color2; + } + + /** + * Sets the LED strip to rainbow pattern. + */ + public void setRainbow() { + currentPattern = LEDPattern.RAINBOW; + } + + /** + * Sets the LED strip to a chasing pattern with the specified color. + * + * @param color Chase color + */ + public void setChase(Color color) { + currentPattern = LEDPattern.CHASE; + primaryColor = color; + } + + /** + * Sets the LED strip to a breathing pattern with the specified color. + * + * @param color Breathing color + */ + public void setBreathing(Color color) { + currentPattern = LEDPattern.BREATHING; + primaryColor = color; + } + + /** + * Sets the LED strip to a blinking pattern with the specified color. + * + * @param color Blinking color + */ + public void setBlinking(Color color) { + currentPattern = LEDPattern.BLINKING; + primaryColor = color; + } + + /** + * Sets the animation speed (time between frames). + * + * @param speedSeconds Time in seconds between animation frames + */ + public void setAnimationSpeed(double speedSeconds) { + this.animationSpeed = speedSeconds; + } + + /** + * Indicates coral intake by setting green chase pattern. + */ + public void indicateIntake() { + setChase(Color.kGreen); + } + + /** + * Indicates coral scoring by setting gold breathing pattern. + */ + public void indicateScoring() { + setBreathing(Color.kGold); + } + + /** + * Indicates error state with red blinking. + */ + public void indicateError() { + setBlinking(Color.kRed); + } + + /** + * Updates the LED strip based on the current pattern. + */ + private void updateAnimation() { + switch (currentPattern) { + case SOLID: + // No animation update needed + break; + + case BREATHING: + updateBreathing(); + break; + + case RAINBOW: + updateRainbow(); + break; + + case CHASE: + updateChase(); + break; + + case ALTERNATING: + updateAlternating(); + break; + + case BLINKING: + updateBlinking(); + break; + } + + // Send the updated data + ledStrip.setData(ledBuffer); + } + + /** + * Updates the breathing animation. + */ + private void updateBreathing() { + // Sine wave for smooth breathing effect + double brightness = (Math.sin(animationCounter * 0.05) + 1.0) / 2.0; + animationCounter++; + + for (var i = 0; i < length; i++) { + ledBuffer.setLED(i, new Color( + primaryColor.red * brightness, + primaryColor.green * brightness, + primaryColor.blue * brightness + )); + } + } + + /** + * Updates the rainbow animation. + */ + private void updateRainbow() { + // For every pixel + for (var i = 0; i < length; i++) { + // Calculate the hue - hue is easier for rainbows because the color + // shape is a circle so only one value needs to change + final var hue = (rainbowFirstPixelHue + (i * 180 / length)) % 180; + // Set the value + ledBuffer.setHSV(i, hue, 255, 128); + } + // Increase by to make the rainbow "move" + rainbowFirstPixelHue += 3; + // Check bounds + rainbowFirstPixelHue %= 180; + } + + /** + * Updates the chase animation. + */ + private void updateChase() { + for (var i = 0; i < length; i++) { + // Set every third pixel to the primary color + if ((i + animationCounter) % 3 == 0) { + ledBuffer.setLED(i, primaryColor); + } else { + ledBuffer.setLED(i, Color.kBlack); + } + } + animationCounter++; + } + + /** + * Updates the alternating animation. + */ + private void updateAlternating() { + boolean useFirstColor = (animationCounter / 10) % 2 == 0; + + for (var i = 0; i < length; i++) { + if (i % 2 == 0) { + ledBuffer.setLED(i, useFirstColor ? primaryColor : secondaryColor); + } else { + ledBuffer.setLED(i, useFirstColor ? secondaryColor : primaryColor); + } + } + + animationCounter++; + } + + /** + * Updates the blinking animation. + */ + private void updateBlinking() { + boolean on = (animationCounter / 10) % 2 == 0; + + for (var i = 0; i < length; i++) { + ledBuffer.setLED(i, on ? primaryColor : Color.kBlack); + } + + animationCounter++; + } + + @Override + public void periodic() { + // Check if it's time for a new animation frame + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastAnimationTime >= animationSpeed) { + updateAnimation(); + lastAnimationTime = currentTime; + } + } +}