diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index 2a8307e..e78ba8d 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -11,9 +11,15 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act import java.lang.reflect.Constructor; import java.lang.reflect.InvocationTargetException; import java.util.HashMap; +import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.CopyOnWriteArrayList; /** + * CyberarmEngine Version 3.0 | December 31st 2022 + * After a few years of use, it's safe to say this implementation is stable and reasonably feature complete. + * * Added support for background tasks that run unqueued for the duration of the op mode unless stopped. + * * Added thread-safe 'blackboard' for storing bits that need to be easily shared between states/tasks. + * * CyberarmEngine Version 2.0 | October 26th 2018 * AN Experimental reimplementation of GoldfishPi's original Engine system. * Designed to be easily maintainable, extensible, and easy to understand. @@ -23,9 +29,12 @@ public abstract class CyberarmEngine extends OpMode { public static CyberarmEngine instance; //Array To Hold States final private CopyOnWriteArrayList cyberarmStates = new CopyOnWriteArrayList<>(); - public HashMap blackboard = new HashMap<>(); - private int activeStateIndex = 0; - private boolean isRunning; + // Array to Hold Tasks + final private CopyOnWriteArrayList backgroundTasks = new CopyOnWriteArrayList<>(); + // HashMap to store data for States and Tasks + private ConcurrentHashMap blackboard = new ConcurrentHashMap<>(); + private int activeStateIndex = 0; // Index of currently running state + private boolean isRunning; // Whether engine is running or not private final static String TAG = "PROGRAM.ENGINE"; public boolean showStateChildrenListInTelemetry = false; @@ -49,6 +58,11 @@ public abstract class CyberarmEngine extends OpMode { for (CyberarmState state: cyberarmStates) { initState(state); } + + // Background tasks + for (CyberarmState task : backgroundTasks) { + initState(task); + } } /** @@ -73,6 +87,11 @@ public abstract class CyberarmEngine extends OpMode { if (cyberarmStates.size() > 0) { runState(cyberarmStates.get(0)); } + + // Background tasks + for (CyberarmState task : backgroundTasks) { + runState(task); + } } /** @@ -95,7 +114,11 @@ public abstract class CyberarmEngine extends OpMode { } // Add telemetry to show currently running state - telemetry.addLine("Running state: " +state.getClass().getSimpleName() + ". State: " + activeStateIndex + " of " + (cyberarmStates.size()-1)); + telemetry.addLine( + "Running state: " +state.getClass().getSimpleName() + ". State: " + + (activeStateIndex + 1) + " of " + (cyberarmStates.size()) + + " (" + activeStateIndex + "/" + (cyberarmStates.size() - 1) + ")"); + if (showStateChildrenListInTelemetry && state.hasChildren()) { for(CyberarmState child: state.children) { telemetry.addLine(" Child: " + child.getClass().getSimpleName() + " [" + child.children.size() + "] grandchildren"); @@ -113,6 +136,11 @@ public abstract class CyberarmEngine extends OpMode { } else { stateTelemetry(state); + + // Background tasks + for (CyberarmState task : backgroundTasks) { + initState(task); + } } gamepadCheckerGamepad1.update(); @@ -128,7 +156,10 @@ public abstract class CyberarmEngine extends OpMode { stopState(state); } - + // Background tasks + for (CyberarmState task : backgroundTasks) { + stopState(task); + } } /** @@ -225,6 +256,76 @@ public abstract class CyberarmEngine extends OpMode { return state; } + /** + * Adds state to the most recently added top level state as a parallel state + * @param state State to add to last top level state + * @return CyberarmState + */ + public CyberarmState addParallelStateToLastState(CyberarmState state) { + CyberarmState parentState = cyberarmStates.get(cyberarmStates.size() - 1); + + Log.i(TAG, "Adding parallel cyberarmState "+ state.getClass() + " to parent state " + parentState.getClass()); + + parentState.addParallelState((state)); + + return state; + } + + /** + * Adds state as a background task that is run until the opmode stops + * background tasks are not queued, they are all started at once. + * @param state State to add to list + * @return CyberarmState + */ + public CyberarmState addTask(CyberarmState state) { + Log.i(TAG, "Adding task cyberarmState "+ state.getClass()); + + backgroundTasks.add(state); + + if (isRunning()) { + initState(state); + runState(state); + } + + return state; + } + + /** + * Retrieve value from blackboard + * @param key String to use to look up value + * @return Returns Object which should be autocast to the correct type + */ + @SuppressWarnings("unchecked") + public T blackboard_get(String key) { + return (T) blackboard.get(key); + } + + /** + * Set value of key to value + * @param key String + * @param value Object + * @return Returns T + */ + @SuppressWarnings("unchecked") + public T blackboard_set(String key, Object value) { + blackboard.put(key, value); + + return (T) value; + } + + /** + * Remove value from blackboard + * @param key String + * @param value Object + * @return Returns T + */ + @SuppressWarnings("unchecked") + public T blackboard_remove(String key, Object value) { + blackboard.remove(key); + + return (T) value; + } + private void buttonDownForStates(CyberarmState state, Gamepad gamepad, String button) { state.buttonDown(gamepad, button); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java index f6d78a9..5f69767 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -96,6 +96,6 @@ public class LeftSideAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java index 34dbdf9..0032c62 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -148,6 +148,6 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java index 249b0ed..c714223 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -97,6 +97,6 @@ public class RightSideAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java index 554034c..6bb512a 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -21,7 +21,7 @@ public class ConeIdentification extends CyberarmState { @Override public void init() { - engine.blackboard.put("parkPlace", "1"); + engine.blackboard_set("parkPlace", "1"); robot.tfod.activate(); initTime = System.currentTimeMillis(); } @@ -50,11 +50,11 @@ public class ConeIdentification extends CyberarmState { engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height); if (recognition.getLabel().equals("#2")) { - engine.telemetry.addData("#2", engine.blackboard.put("parkPlace", "2")); + engine.telemetry.addData("#2", engine.blackboard_set("parkPlace", "2")); } else if (recognition.getLabel().equals("#3")) { - engine.telemetry.addData("#3",engine.blackboard.put("parkPlace", "3")); + engine.telemetry.addData("#3",engine.blackboard_set("parkPlace", "3")); } else { - engine.telemetry.addData("#1", engine.blackboard.put("parkPlace", "1")); + engine.telemetry.addData("#1", engine.blackboard_set("parkPlace", "1")); } } } @@ -90,12 +90,12 @@ public class ConeIdentification extends CyberarmState { bestConfidence = recognition.getConfidence(); if (recognition.getLabel().equals("2 Bulb")) { - engine.blackboard.put("parkPlace", "2"); + engine.blackboard_set("parkPlace", "2"); } else if (recognition.getLabel().equals("3 Panel")) { - engine.blackboard.put("parkPlace", "3"); + engine.blackboard_set("parkPlace", "3"); } else { - engine.blackboard.put("parkPlace", "1"); + engine.blackboard_set("parkPlace", "1"); } } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java index 816e91c..f1dd8cf 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -43,7 +43,7 @@ public class DriverParkPlaceState extends CyberarmState { setHasFinished(true); return; } - String placement = engine.blackboard.get("parkPlace"); + String placement = engine.blackboard_get("parkPlace"); if (placement != null) { if (!placement.equals(intendedPlacement)){ setHasFinished(true); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java index 051f527..483468d 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -16,7 +16,7 @@ public class PathDecision extends CyberarmState { @Override public void exec() { - String placement = engine.blackboard.get("parkPlace"); + String placement = engine.blackboard_get("parkPlace"); setHasFinished(true); }