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..8fc7885 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -10,10 +10,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 +28,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 + final 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 +57,11 @@ public abstract class CyberarmEngine extends OpMode { for (CyberarmState state: cyberarmStates) { initState(state); } + + // Background tasks + for (CyberarmState task : backgroundTasks) { + initState(task); + } } /** @@ -73,6 +86,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 +113,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 +135,11 @@ public abstract class CyberarmEngine extends OpMode { } else { stateTelemetry(state); + + // Background tasks + for (CyberarmState task : backgroundTasks) { + stateTelemetry(task); + } } gamepadCheckerGamepad1.update(); @@ -128,7 +155,10 @@ public abstract class CyberarmEngine extends OpMode { stopState(state); } - + // Background tasks + for (CyberarmState task : backgroundTasks) { + stopState(task); + } } /** @@ -225,6 +255,98 @@ 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 T of stored Object + */ + public T blackboardGet(String key) { + return (T) blackboard.get(key); + } + + public String blackboardGetString(String key) { + return (String) blackboard.get(key); + } + + public int blackboardGetInt(String key) { + return (int) blackboard.get(key); + } + + public long blackboardGetLong(String key) { + return (long) blackboard.get(key); + } + + public float blackboardGetFloat(String key) { + return (float) blackboard.get(key); + } + + public double blackboardGetDouble(String key) { + return (double) blackboard.get(key); + } + + public boolean blackboardGetBoolean(String key) { + return (boolean) blackboard.get(key); + } + + /** + * Set value of key to value + * @param key String + * @param value Object + * @return Returns T + */ + + public T blackboardSet(String key, T value) { + blackboard.put(key, value); + + return (T) value; + } + + /** + * Remove value from blackboard + * @param key String + * @param value Object + * @return Returns T + */ + public T blackboardRemove(String key, T 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..38559f1 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", blackboardGetString("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..f2a8dc2 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", blackboardGetString("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..fef79d6 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", blackboardGetString("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..43c6473 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.blackboardSet("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.blackboardSet("parkPlace", "2")); } else if (recognition.getLabel().equals("#3")) { - engine.telemetry.addData("#3",engine.blackboard.put("parkPlace", "3")); + engine.telemetry.addData("#3",engine.blackboardSet("parkPlace", "3")); } else { - engine.telemetry.addData("#1", engine.blackboard.put("parkPlace", "1")); + engine.telemetry.addData("#1", engine.blackboardSet("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.blackboardSet("parkPlace", "2"); } else if (recognition.getLabel().equals("3 Panel")) { - engine.blackboard.put("parkPlace", "3"); + engine.blackboardSet("parkPlace", "3"); } else { - engine.blackboard.put("parkPlace", "1"); + engine.blackboardSet("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..382615f 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.blackboardGetString("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..87923bf 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.blackboardGetString("parkPlace"); setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java new file mode 100644 index 0000000..eea9c75 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java @@ -0,0 +1,76 @@ +package org.timecrafters.minibots.cyberarm.engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.cyberarm.engine.V2.CyberarmState; + +@TeleOp(name = "TasksEngine", group = "testing") +public class TasksEngine extends CyberarmEngine { + @Override + public void setup() { + addTask(new CyberarmState() { + @Override + public void init() { + engine.blackboardSet("counter", 0); + engine.blackboardSet("string", "I IS STRING"); + engine.blackboardSet("boolean", true); + } + + @Override + public void exec() { + } + + @Override + public void telemetry() { + engine.telemetry.addData("TASK 1", engine.blackboardGetInt("counter")); + engine.telemetry.addData("TASK 1", "I am a task!"); + } + }); + + addTask(new CyberarmState() { + int lastCount = 0, blackboardLastCount = 0; + int count = 0; + double lastRuntime = 0; + @Override + public void exec() { + if (runTime() - lastRuntime >= 1000.0) { + lastRuntime = runTime(); + lastCount = count; + blackboardLastCount = engine.blackboardGetInt("counter"); + } + engine.blackboardSet("counter", engine.blackboardGetInt("counter") + 1); + count++; + } + + @Override + public void telemetry() { + engine.telemetry.addData("TASK 2", engine.blackboardGetString("string")); + engine.telemetry.addData("TASK 2", engine.blackboardGetBoolean("boolean")); + engine.telemetry.addData("TASK 2", engine.blackboardGetInt("counter") - blackboardLastCount); + engine.telemetry.addData("TASK 2", count - lastCount); + engine.telemetry.addData("TASK 2", engine.blackboardGetString("string_NULL") == null); + } + }); + + addState(new CyberarmState() { + @Override + public void exec() { + if (runTime() >= 10_000) { + setHasFinished(true); + } + } + }); + + addParallelStateToLastState(new CyberarmState() { + @Override + public void exec() { + } + + @Override + public void telemetry() { + engine.telemetry.addData("Parallel state 1", "Hello There"); + } + }); + } +}