Merge remote-tracking branch 'origin/master'

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<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
-    public HashMap<String, String> blackboard = new HashMap<>();
+  // Array to Hold Tasks
-    private int activeStateIndex = 0;
+  final private CopyOnWriteArrayList<CyberarmState> backgroundTasks = new CopyOnWriteArrayList<>();
-  private boolean isRunning;
+  // HashMap to store data for States and Tasks
+  final private ConcurrentHashMap<String, Object> 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> 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> 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> 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);
 

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"));
     }
 }

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"));
     }
 }

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"));
     }
 }

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");
                         }
                     }
                 }

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);

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);
         }
 

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");
+            }
+        });
+    }
+}