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

@@ -151,6 +151,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/TeleOp/states/PhoenixTeleOPState.java

@@ -20,7 +20,15 @@ public class PhoenixTeleOPState extends CyberarmState {
     private int CyclingArmUpAndDown = 0;
     private double RobotRotation;
     private double RotationTarget, DeltaRotation;
-    private double MinimalPower = 0.25;
+    private double MinimalPower = 0.25, topServoOffset = -0.05;
+    private double servoCollectLow = 0.45; //Low servos, A button
+    private double servoCollectHigh = 0.55; //High servos, A button
+    private double servoLowLow = 0.45; //Low servos, X button
+    private double servoLowHigh = 0.75; //High servos, X button
+    private double servoMedLow = 0.45; //Low servos, B button
+    private double servoMedHigh = 0.9; //High servos, B button
+    private double servoHighLow = 0.8; //Low servos, Y button
+    private double servoHighHigh = 0.9; //High servos, Y button
     private GamepadChecker gamepad1Checker, gamepad2Checker;
     private int OCD;
 
@@ -68,7 +76,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         robot.LowRiserLeft.setPosition(0.45);
         robot.LowRiserRight.setPosition(0.45);
         robot.HighRiserLeft.setPosition(0.45);
-        robot.HighRiserRight.setPosition(0.45);
+        robot.HighRiserRight.setPosition(0.45 + topServoOffset);
         robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
@@ -123,24 +131,78 @@ public class PhoenixTeleOPState extends CyberarmState {
                 Math.abs(engine.gamepad1.left_stick_y) < 0.1 &&
                 Math.abs(engine.gamepad1.right_stick_y) < 0.1) {
             drivePower = 0;
-            robot.backLeftDrive.setPower(-drivePower);
+            robot.backLeftDrive.setPower(drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+        }
+
+        if (engine.gamepad1.dpad_right) {
+            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RotationTarget = 90;
+            CalculateDeltaRotation();
+            if (RobotRotation > -90 && RobotRotation < 89) {//CCW
+                drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower;
+                robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
-            robot.frontLeftDrive.setPower(-drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
+            if (RobotRotation > 91 || RobotRotation < -90) {//CW
+                drivePower = (0.95 * DeltaRotation / 180) + MinimalPower;
+                robot.backLeftDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(-drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(-drivePower);
+            }
+            if (RobotRotation < 91 && RobotRotation > 89) {
+                drivePower = 0;
+                robot.backLeftDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(drivePower);
+            }
+        }
+
+        if (engine.gamepad1.dpad_left) {
+            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RotationTarget = -90;
+            CalculateDeltaRotation();
+            if (RobotRotation > -89 && RobotRotation <= 90) {//CW
+                drivePower = (0.95 * DeltaRotation / 180) - MinimalPower;
+                robot.backLeftDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(-drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(-drivePower);
+            }
+            if (RobotRotation > 90 || RobotRotation < -91) {//CCW
+                drivePower = (-0.95 * DeltaRotation / 180) + MinimalPower;
+                robot.backLeftDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(-drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(-drivePower);
+            }
+            if (RobotRotation < 91 && RobotRotation > 89) {
+                drivePower = 0;
+                robot.backLeftDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(drivePower);
+            }
+        }
 
         if (engine.gamepad1.y) {
             RobotRotation = robot.imu.getAngularOrientation().firstAngle;
             RotationTarget = 180;
             CalculateDeltaRotation();
             if (RobotRotation < 0 && RobotRotation > -179) {
-                drivePower = (1 * DeltaRotation / 180) + MinimalPower;
+                drivePower = (0.95 * DeltaRotation / 180) + MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             } else if (RobotRotation >= 0) {
-                drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
+                drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -159,14 +221,14 @@ public class PhoenixTeleOPState extends CyberarmState {
             RotationTarget = 0;
             CalculateDeltaRotation();
             if (RobotRotation < -1) {
-                drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
+                drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
             if (RobotRotation > 1) {
-                drivePower = (1 * DeltaRotation / 180) + MinimalPower;
+                drivePower = (0.95 * DeltaRotation / 180) + MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -186,14 +248,14 @@ public class PhoenixTeleOPState extends CyberarmState {
             RotationTarget = -45;
             CalculateDeltaRotation();
             if (RobotRotation < -46 || RobotRotation > 135) {//CCW
-                drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
+                drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
             if (RobotRotation > -44 && RobotRotation < 135) {//CW
-                drivePower = (1 * DeltaRotation / 180) + MinimalPower;
+                drivePower = (0.95 * DeltaRotation / 180) + MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -202,36 +264,9 @@ public class PhoenixTeleOPState extends CyberarmState {
             if (RobotRotation > -46 && RobotRotation < -44) {
                 drivePower = 0;
                 robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(-drivePower);
+                robot.backRightDrive.setPower(drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(-drivePower);
+                robot.frontRightDrive.setPower(drivePower);
-            }
-        }
-
-        if (engine.gamepad1.dpad_right) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
-            RotationTarget = 45;
-            CalculateDeltaRotation();
-            if (RobotRotation > -135 && RobotRotation < 44) {//CCW
-                drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
-                robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(-drivePower);
-                robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(-drivePower);
-            }
-            if (RobotRotation < -135 || RobotRotation < 46) {//CW
-                drivePower = (1 * DeltaRotation / 180) + MinimalPower;
-                robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(-drivePower);
-                robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(-drivePower);
-            }
-            if (RobotRotation < 46 && RobotRotation > 44) {
-                drivePower = 0;
-                robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(-drivePower);
-                robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(-drivePower);
             }
         }
 
@@ -240,14 +275,14 @@ public class PhoenixTeleOPState extends CyberarmState {
             RotationTarget = 45;
             CalculateDeltaRotation();
             if (RobotRotation > -135 && RobotRotation < 44) {//CCW
-                drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
+                drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
             if (RobotRotation < -135 || RobotRotation > 46) {//CW
-                drivePower = (1 * DeltaRotation / 180) + MinimalPower;
+                drivePower = (0.95 * DeltaRotation / 180) + MinimalPower;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -256,9 +291,9 @@ public class PhoenixTeleOPState extends CyberarmState {
             if (RobotRotation < 46 && RobotRotation > 44) {
                 drivePower = 0;
                 robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(-drivePower);
+                robot.backRightDrive.setPower(drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(-drivePower);
+                robot.frontRightDrive.setPower(drivePower);
             }
         }
 
@@ -274,7 +309,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad2.right_bumper) {
-            if (robot.HighRiserLeft.getPosition() < 1.0) {
+            if (robot.HighRiserLeft.getPosition() < 1.0 - Math.abs(topServoOffset)) {
                 if (System.currentTimeMillis() - lastStepTime >= 150) {
                     lastStepTime = System.currentTimeMillis();
                     OCD = 0;
@@ -304,7 +339,7 @@ public class PhoenixTeleOPState extends CyberarmState {
                 }
             }
 
-            if (/*robot.LowRiserLeft.getPosition() < 0.75 &&*/ robot.HighRiserLeft.getPosition() > 0.85 && downSensor() < 840) {
+            if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.85) {
                 if (System.currentTimeMillis() - lastStepTime >= 150) {
                     lastStepTime = System.currentTimeMillis();
                     robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
@@ -331,49 +366,8 @@ public class PhoenixTeleOPState extends CyberarmState {
             }
         }//end of a
 
-//        if (engine.gamepad2.back) {
+        //i feel like removing that was a stupidly impulsive move but i gave up on not being stupidly
-//            robot.backLeftDrive.setPower(1);
+        // impulsive a long time ago lol. Besides, when have we even used that function? It got replaced.
-//            robot.backRightDrive.setPower(1);
-//            robot.frontLeftDrive.setPower(1);
-//            robot.frontRightDrive.setPower(1);
-//            if (System.currentTimeMillis() - lastStepTime >= 1500) {
-//                robot.backLeftDrive.setPower(0);
-//                robot.backRightDrive.setPower(0);
-//                robot.frontLeftDrive.setPower(0);
-//                robot.frontRightDrive.setPower(0);
-//            }
-//            if (System.currentTimeMillis() - lastStepTime >= 150) {
-//                if (robot.HighRiserLeft.getPosition() < 1) {
-//                    if (System.currentTimeMillis() - lastStepTime >= 150) {
-//                        lastStepTime = System.currentTimeMillis();
-//                        robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
-//                        robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
-//                    }
-//                }
-//            }
-//            if (System.currentTimeMillis() - lastStepTime >= 150) {
-//                if (robot.LowRiserLeft.getPosition() < 1 && robot.HighRiserLeft.getPosition() == 1) {
-//                    if (System.currentTimeMillis() - lastStepTime >= 150) {
-//                        lastStepTime = System.currentTimeMillis();
-//                        robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-//                        robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-//                    }
-//                }
-//            }
-//            if (System.currentTimeMillis() >= 250) {
-//                robot.backLeftDrive.setPower(1);
-//                robot.backRightDrive.setPower(1);
-//                robot.frontLeftDrive.setPower(1);
-//                robot.frontRightDrive.setPower(1);
-//                if (System.currentTimeMillis() - lastStepTime >= 250) {
-//                    robot.backLeftDrive.setPower(0);
-//                    robot.backRightDrive.setPower(0);
-//                    robot.frontLeftDrive.setPower(0);
-//                    robot.frontRightDrive.setPower(0);
-//                }
-//            }
-//
-//        }
 
         if (engine.gamepad2.a) {
             OCD = 1;
@@ -391,108 +385,115 @@ public class PhoenixTeleOPState extends CyberarmState {
             OCD = 4;
         }
 
-        if (OCD == 1) { //Ground junction
+        if (OCD == 1) { //Ground junction/Collect
-            if (robot.LowRiserLeft.getPosition() >= 0.46) {
+            if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 0.01) {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
                     robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
                 }
-            } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.46) {
+            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                        robot.HighRiserLeft.getPosition() > servoCollectHigh) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
                 }
-            } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() <= 0.46) {
+            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 0.01 &&
+                        robot.HighRiserLeft.getPosition() <= servoCollectHigh) {
                 OCD = 0;
             }
         }
 
         if (OCD == 2) { //low junction
-            if (robot.LowRiserLeft.getPosition() > 0.46 && robot.HighRiserLeft.getPosition() > 0.65) {
+            if (robot.LowRiserLeft.getPosition() > servoLowLow + 0.01) {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
                     robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
                 }
-            } // <-- both levels too high
+            } // <-- low level too high
-            if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.LowRiserLeft.getPosition() > 0.44 && robot.HighRiserLeft.getPosition() > 0.66) {
+            if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 &&
+                robot.HighRiserLeft.getPosition() > servoLowHigh) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
                 }
             } // <-- top level too high
-            if (robot.LowRiserLeft.getPosition() < 0.44 && robot.HighRiserLeft.getPosition() < 0.64) {
+            if (robot.LowRiserLeft.getPosition() < servoLowLow - 0.01) {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
                     robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
                 }
-            } // <-- both levels too low
+            } // <-- low level too low
-            if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.64) {
+            if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                robot.HighRiserLeft.getPosition() < servoLowHigh - 0.01) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
                 }
             } // <-- high level too low
-            if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.64 && robot.HighRiserLeft.getPosition() <= 0.66) {
+            if (robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 &&
+                robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
+                robot.HighRiserLeft.getPosition() > servoLowHigh - 0.01 &&
+                robot.HighRiserLeft.getPosition() <=  servoLowHigh + 0.01) {
                 OCD = 0;
             }
         }
 
         if (OCD == 3) { // Medium junction
-            if (robot.LowRiserLeft.getPosition() > 0.46 && robot.HighRiserLeft.getPosition() > 0.8) {
+            if (robot.LowRiserLeft.getPosition() > servoMedLow + 0.01) {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
                     robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
                 }
-            } // <-- both levels too high
+            } // <-- low level too high
-            if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.8) {
+            if (robot.LowRiserLeft.getPosition() <= servoMedLow + 0.01 &&
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                robot.HighRiserLeft.getPosition() > servoMedHigh + 0.01)/* <-- high level too high*/ {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
                 }
-            } // <-- top level too high
-            if (robot.LowRiserLeft.getPosition() < 0.44 && robot.HighRiserLeft.getPosition() < 0.79) {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
             }
-            } // <-- both levels too low
+            if (robot.LowRiserLeft.getPosition() < servoMedLow + 0.01 &&
-            if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.79) {
+                robot.HighRiserLeft.getPosition() < servoMedHigh - 0.01)/* <-- high level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
                 }
-            } // <-- high level too low
+            }
-            if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.6 && robot.HighRiserLeft.getPosition() > 0.79 && robot.HighRiserLeft.getPosition() <= 0.81) {
+            if (robot.LowRiserLeft.getPosition() > servoMedLow - 0.01 &&
+                robot.LowRiserLeft.getPosition() <= servoMedLow &&
+                robot.HighRiserLeft.getPosition() > servoMedHigh - 0.01 &&
+                robot.HighRiserLeft.getPosition() <= servoMedHigh) {
                 OCD = 0;
             }
         }
 
         if (OCD == 4) { // High Junction
-            if (robot.HighRiserLeft.getPosition() < 0.84) {
+            if (robot.HighRiserLeft.getPosition() < servoHighHigh - 0.01) {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
                 }
             }
-            if (robot.LowRiserLeft.getPosition() <= 0.64) {
+            if (robot.LowRiserLeft.getPosition() < servoHighLow - 0.01) {
-                if (System.currentTimeMillis() - lastStepTime >= 125) {
+                if (System.currentTimeMillis() - lastStepTime >= 120) {
                     lastStepTime = System.currentTimeMillis();
                     robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
                     robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
                 }
             }
-            if (robot.HighRiserLeft.getPosition() >= 0.84 && robot.LowRiserLeft.getPosition() >= 0.64) {
+            if (robot.HighRiserLeft.getPosition() >= servoHighHigh &&
+                robot.LowRiserLeft.getPosition() >= servoHighLow) {
                 OCD = 0;
             }
         }
@@ -516,18 +517,13 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
     }
     public double downSensor() {
-        double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5, Distance_6, Distance_7, Distance_8, Distance_9, Distance_10;
+        double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
         Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
         Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
         Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
         Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
         Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_6 = robot.downSensor.getDistance(DistanceUnit.MM);
+        Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
-//        Distance_7 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_8 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_9 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_10 = robot.downSensor.getDistance(DistanceUnit.MM);
-        Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5/* + Distance_6 + Distance_7 + Distance_8 + Distance_9 + Distance_10*/)/5;
         return Distance;
 
     }

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