diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 6b35533..66a4bb2 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -92,7 +92,12 @@ public class RedCrabMinibot { public final DigitalChannel ledTopGreen, ledTopRed, ledRail0Green, ledRail0Red, ledRail1Green, ledRail1Red, ledRail2Green, ledRail2Red, ledRail3Green, ledRail3Red; public final ArrayList railGreenLEDs = new ArrayList<>(), railRedLEDs = new ArrayList<>(); - public final boolean LED_OFF = true, LED_ON = false; + public int ledChaserIndex = 0; + public final long ledChaserIntervalMS; + public long lastLedChaserTimeMS; + public final boolean LED_OFF = true; + public final boolean LED_ON = false; + public boolean ledChaseUp = true; final CyberarmEngine engine; public final boolean autonomous; @@ -299,6 +304,9 @@ public class RedCrabMinibot { led.setState(LED_ON); } + ledChaserIntervalMS = 250; + lastLedChaserTimeMS = System.currentTimeMillis(); + // Bulk read from hubs Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL); @@ -625,20 +633,22 @@ public class RedCrabMinibot { ledTopRed.setState(LED_OFF); ledTopGreen.setState(LED_ON); - ledSetRailRedLEDs(LED_OFF); - ledSetRailGreenLEDs(LED_ON); - } else if (engine.runTime() < 25_000.0) { // RUNNING LOW ON TIME - ledTopRed.setState(LED_ON); + ledAnimateChaser(LED_OFF, LED_ON, false); + } else if (engine.runTime() < 23_000.0) { // RUNNING LOW ON TIME + ledTopRed.setState(LED_OFF); ledTopGreen.setState(LED_ON); - ledSetRailRedLEDs(LED_ON); - ledSetRailGreenLEDs(LED_ON); + ledAnimateChaser(LED_ON, LED_ON, false); } else { // 5 SECONDS LEFT! - ledTopRed.setState(LED_ON); - ledTopGreen.setState(LED_OFF); + if (engine.runTime() < 29_000.0) { + ledTopRed.setState(LED_ON); + ledTopGreen.setState(LED_ON); + } else { + ledTopRed.setState(LED_ON); + ledTopGreen.setState(LED_OFF); + } - ledSetRailRedLEDs(LED_ON); - ledSetRailGreenLEDs(LED_OFF); + ledAnimateProgress(LED_ON, LED_OFF, (engine.runTime() - 23_000.0) / 5_000.0); } } else { if (engine.runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP @@ -648,17 +658,20 @@ public class RedCrabMinibot { ledSetRailRedLEDs(LED_OFF); ledSetRailGreenLEDs(LED_ON); } else if (engine.runTime() >= 80_000.0) { // GET READY - ledTopRed.setState(LED_ON); - ledTopGreen.setState(LED_ON); + if (ledChaserIndex == railRedLEDs.size() - 1) { + ledTopRed.setState(LED_OFF); + ledTopGreen.setState(LED_OFF); + } else { + ledTopRed.setState(LED_ON); + ledTopGreen.setState(LED_ON); + } - ledSetRailRedLEDs(LED_ON); - ledSetRailGreenLEDs(LED_ON); + ledAnimateChaser(LED_ON, LED_ON, false); } else { // KEEP CALM and CARRY ON ledTopRed.setState(LED_ON); ledTopGreen.setState(LED_OFF); - ledSetRailRedLEDs(LED_ON); - ledSetRailGreenLEDs(LED_OFF); + ledAnimateChaser(LED_ON, LED_OFF, false); } } } @@ -674,4 +687,70 @@ public class RedCrabMinibot { led.setState(ledState); } } + + public void ledAnimateChaser(boolean ledRed, boolean ledGreen, boolean invert) { + if (System.currentTimeMillis() - lastLedChaserTimeMS >= ledChaserIntervalMS) { + lastLedChaserTimeMS = System.currentTimeMillis(); + } else { + return; + } + + // Turn off current LED + if (invert) { + railRedLEDs.get(ledChaserIndex).setState(ledRed); + railGreenLEDs.get(ledChaserIndex).setState(ledGreen); + } else { + railRedLEDs.get(ledChaserIndex).setState(LED_OFF); + railGreenLEDs.get(ledChaserIndex).setState(LED_OFF); + } + + // Cycle up/down + if (ledChaseUp) { + ledChaserIndex++; + } else { + ledChaserIndex--; + } + + // Switch active LED + if (ledChaserIndex >= railRedLEDs.size()) { + ledChaseUp = !ledChaseUp; + ledChaserIndex = railRedLEDs.size() - 2; + } else if (ledChaserIndex < 0) { + ledChaseUp = !ledChaseUp; + ledChaserIndex = 1; + } + + // Control active LED + if (!invert) { + railRedLEDs.get(ledChaserIndex).setState(ledRed); + railGreenLEDs.get(ledChaserIndex).setState(ledGreen); + } else { + railRedLEDs.get(ledChaserIndex).setState(LED_OFF); + railGreenLEDs.get(ledChaserIndex).setState(LED_OFF); + } + } + + public void ledAnimateProgress(boolean progressLEDsRed, boolean progressLEDsGreen, double ratio) { + double i = 0.0; + for (DigitalChannel led : railRedLEDs) { + i += 1.0 / railRedLEDs.size(); + + if (ratio < i) { + led.setState(LED_OFF); + } else { + led.setState(progressLEDsRed); + } + } + + i = 0.0; + for (DigitalChannel led : railGreenLEDs) { + i += 1.0 / railGreenLEDs.size(); + + if (ratio < i) { + led.setState(LED_OFF); + } else { + led.setState(progressLEDsGreen); + } + } + } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/LEDControllerTask.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/LEDControllerTask.java new file mode 100644 index 0000000..9fae11e --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/LEDControllerTask.java @@ -0,0 +1,16 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class LEDControllerTask extends CyberarmState { + private final RedCrabMinibot robot; + public LEDControllerTask(RedCrabMinibot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.ledController(); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java index 7d121a6..64e48ba 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java @@ -14,6 +14,7 @@ public class RedCrabAutonomousBlueAudienceEngine extends RedCrabEngine { public void setup() { robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); + addTask(new LEDControllerTask(robot)); addTask(new LocalizerTask(robot)); setupFromConfig( diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java index e43523e..f915c82 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java @@ -14,6 +14,7 @@ public class RedCrabAutonomousBlueBackstageEngine extends RedCrabEngine { public void setup() { robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); + addTask(new LEDControllerTask(robot)); addTask(new LocalizerTask(robot)); setupFromConfig( diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java index c137332..9df7adf 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java @@ -14,6 +14,7 @@ public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabEngine { public void setup() { robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); + addTask(new LEDControllerTask(robot)); addTask(new LocalizerTask(robot)); setupFromConfig( diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java index 6e0b0f8..fc6c3db 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java @@ -14,6 +14,7 @@ public class RedCrabAutonomousRedAudienceEngine extends RedCrabEngine { public void setup() { robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); + addTask(new LEDControllerTask(robot)); addTask(new LocalizerTask(robot)); setupFromConfig( diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java index ffe38b6..0099695 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java @@ -14,6 +14,7 @@ public class RedCrabAutonomousRedBackstageEngine extends RedCrabEngine { public void setup() { robot = new RedCrabMinibot(true); addTask(new ClawArmTask(robot)); + addTask(new LEDControllerTask(robot)); addTask(new LocalizerTask(robot)); setupFromConfig( diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java index a2ea763..046641f 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java @@ -14,8 +14,6 @@ public abstract class RedCrabEngine extends CyberarmEngine { super.loop(); robot.standardTelemetry(); - - robot.ledController(); } @Override diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java index 116c726..f9c5c52 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java @@ -18,6 +18,7 @@ public class RedCrabTeleOpDebuggingOnlyEngine extends RedCrabEngine { robot = new RedCrabMinibot(false); + addTask(new LEDControllerTask(robot)); addTask(new LocalizerTask(robot)); addState(new CyberarmState() { diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java index 05eebda..c7d3a5b 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java @@ -16,6 +16,7 @@ public class RedCrabTeleOpEngine extends RedCrabEngine { robot = new RedCrabMinibot(false); addTask(new ClawArmTask(robot)); + addTask(new LEDControllerTask(robot)); addTask(new LocalizerTask(robot)); addState(new Pilot(robot));