Initial implementation of camera, changed access of CyberarmEngine#setupFromConfig to public from protected, refactored autonomous engines to only use the 2 logical sides instead of the 4 for states, a hardware fault during autonomous will cause the opmode to abort, misc. changes.

TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java

@@ -415,7 +415,7 @@ public abstract class CyberarmEngine extends OpMode {
    * @param objectClass Class to cast object to
    * @param groupName Group name
    */
-  protected void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class<?> objectClass, String groupName) {
+  public void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class<?> objectClass, String groupName) {
     CyberarmState lastState = null;
     String lastActionName = null;
     String[] lastActionNameSplit = new String[0];

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java

@@ -17,9 +17,13 @@ import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.hardware.ServoImplEx;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.firstinspires.ftc.robotcore.external.ClassFactory;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
@@ -43,6 +47,7 @@ public class Robot {
     public boolean wristManuallyControlled = false;
     public boolean automaticAntiTipActive = false;
     public boolean hardwareFault = false;
+    public String hardwareFaultMessage = "";
 
     private Status status = Status.OKAY, lastStatus = Status.OKAY;
     private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
@@ -82,6 +87,11 @@ public class Robot {
     private WristPosition wristTargetPosition, wristCurrentPosition;
     private double wristPositionChangeTime, wristPositionChangeRequestTime;
 
+    private static final String VUFORIA_KEY =
+            "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
+    private final VuforiaLocalizer vuforia;
+    private final TFObjectDetector tfod;
+
     private boolean LEDStatusToggle = false;
     private double lastLEDStatusAnimationTime = 0;
 
@@ -198,11 +208,38 @@ public class Robot {
             expansionHub.setPattern(ledPatternOkay());
         }
 
+        // Webcam
+        vuforia = initVuforia();
+        tfod = initTfod();
+
         // INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes
         this.fieldLocalizer.setRobot(this);
         this.fieldLocalizer.standardSetup();
     }
 
+    private VuforiaLocalizer initVuforia() {
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+
+        parameters.vuforiaLicenseKey = VUFORIA_KEY;
+        parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
+
+         return ClassFactory.getInstance().createVuforia(parameters);
+    }
+
+    private TFObjectDetector initTfod() {
+        int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        tfodParameters.minResultConfidence = 0.75f;
+        tfodParameters.isModelTensorFlow2 = true;
+        tfodParameters.inputSize = 300;
+        TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
+
+        tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel");
+
+        return tfod;
+    }
+
     public void standardTelemetry() {
         engine.telemetry.addLine();
 
@@ -210,6 +247,7 @@ public class Robot {
         engine.telemetry.addLine("DATA");
         engine.telemetry.addData("      Robot Status", status);
         engine.telemetry.addData("      Hardware Fault", hardwareFault);
+        engine.telemetry.addData("      Hardware Fault Message", hardwareFaultMessage);
         engine.telemetry.addLine();
 
         // Motor Powers
@@ -392,8 +430,11 @@ public class Robot {
             wrist.setPower(0);
         }
 
-        if (getVoltage() < 9.75) {
+        double voltage = getVoltage();
+        if (voltage < 9.75) {
             reportStatus(Status.DANGER);
+            hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)";
+
             hardwareFault = true;
         }
 
@@ -528,6 +569,14 @@ public class Robot {
 
     public double getDiameter() { return diameter; }
 
+    public double getVoltage() {
+        return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
+    }
+
+    public TFObjectDetector getTfod() { return tfod; }
+
+    public VuforiaLocalizer getVuforia() { return vuforia; }
+
     public TimeCraftersConfiguration getConfiguration() { return configuration; }
 
     public FieldLocalizer getFieldLocalizer() { return fieldLocalizer; }
@@ -667,10 +716,6 @@ public class Robot {
         arm.setPower(0.75);
     }
 
-    public double getVoltage() {
-        return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
-    }
-
     public double facing() {
         double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java

@@ -1,11 +1,14 @@
 package org.timecrafters.minibots.cyberarm.chiron.engines;
 
+import android.util.Log;
+
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
 import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
 
 public class AutonomousEngine extends CyberarmEngine {
+    private static final String TAG = "CHIRON|AutonomousEngine";
     protected Robot robot;
     protected FieldLocalizer fieldLocalizer;
     protected TimeCraftersConfiguration configuration;
@@ -49,6 +52,12 @@ public class AutonomousEngine extends CyberarmEngine {
         super.loop();
 
         robot.standardTelemetry();
+
+        if (robot.hardwareFault) {
+            Log.e(TAG, "Hardware fault detected! Aborting run. Message: " + robot.hardwareFaultMessage);
+
+            stop();
+        }
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
 public class AutonomousBlueLeftSideEngine extends AutonomousEngine {
     @Override
     public void setup() {
-        actionsGroupName = "AutonomousBlueLeftSide";
+        actionsGroupName = "AutonomousLeftSide";
         tuningGroupName = "Autonomous";
         tuningActionName = "Tuning_Blue_LeftSide";
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
 public class AutonomousBlueRightSideEngine extends AutonomousEngine {
     @Override
     public void setup() {
-        actionsGroupName = "AutonomousBlueRightSide";
+        actionsGroupName = "AutonomousRightSide";
         tuningGroupName = "Autonomous";
         tuningActionName = "Tuning_Blue_RightSide";
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
 public class AutonomousRedLeftSideEngine extends AutonomousEngine {
     @Override
     public void setup() {
-       actionsGroupName = "AutonomousRedLeftSide";
+       actionsGroupName = "AutonomousLeftSide";
         tuningGroupName = "Autonomous";
         tuningActionName = "Tuning_Red_LeftSide";
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java

@@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine;
 public class AutonomousRedRightSideEngine extends AutonomousEngine {
     @Override
     public void setup() {
-        actionsGroupName = "AutonomousRedRightSide";
+        actionsGroupName = "AutonomousRightSide";
         tuningGroupName = "Autonomous";
         tuningActionName = "Tuning_Red_RightSide";
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java

@@ -7,15 +7,45 @@ public class SelectParkingPosition extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
 
+    private final double timeInMS;
+    private final boolean stateDisabled;
+
     public SelectParkingPosition(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
         this.actionName = actionName;
+
+        timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
+
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+    }
+
+    @Override
+    public void start() {
+        int pos = engine.blackboardGetInt("parking_position");
+
+        engine.setupFromConfig(
+                robot.getConfiguration(),
+                "org.timecrafters.minibots.cyberarm.chiron.states.autonomous",
+                robot,
+                Robot.class,
+                "" + groupName + "_Parking_" + pos);
     }
 
     @Override
     public void exec() {
-        // FIXME: NO OP
+        if (stateDisabled) {
             setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
     }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java

@@ -1,26 +1,109 @@
 package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
 
+import java.util.List;
+
 public class SignalProcessor extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
 
+    private final double timeInMS, minConfidence;
+    private final int fallbackPosition;
+    private final boolean stateDisabled;
+
     public SignalProcessor(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
         this.actionName = actionName;
+
+        timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
+        minConfidence = robot.getConfiguration().variable(groupName, actionName, "minConfidence").value();
+        fallbackPosition = robot.getConfiguration().variable(groupName, actionName, "fallbackPosition").value();
+
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
     }
 
     @Override
     public void start() {
-        engine.blackboardSet("parking_position", System.currentTimeMillis() % 3);
+        engine.blackboardSet("parking_position", fallbackPosition);
+
+        robot.getTfod().activate();
     }
 
     @Override
     public void exec() {
-        // FIXME: NO OP
+        if (stateDisabled) {
+            stop();
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            return;
+        }
+
+        if (robot.getTfod() != null) {
+            // getUpdatedRecognitions() will return null if no new information is available since
+            // the last time that call was made.
+            List<Recognition> updatedRecognitions = robot.getTfod().getUpdatedRecognitions();
+
+            if (updatedRecognitions != null) {
+                for (Recognition recognition : updatedRecognitions) {
+                    switch (recognition.getLabel()) {
+                        case "1 Bolt":
+                            engine.blackboardSet("parking_position", 1);
+
+                            break;
+                        case "2 Bulb":
+                            engine.blackboardSet("parking_position", 2);
+
+                            break;
+                        case "3 Panel":
+                            engine.blackboardSet("parking_position", 3);
+
+                            break;
+                    }
+                }
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        if (robot.getTfod() != null) {
+            // getUpdatedRecognitions() will return null if no new information is available since
+            // the last time that call was made.
+            List<Recognition> updatedRecognitions = robot.getTfod().getUpdatedRecognitions();
+
+            if (updatedRecognitions != null) {
+                engine.telemetry.addData("# Objects Detected", updatedRecognitions.size());
+
+                // step through the list of recognitions and display image position/size information for each one
+                // Note: "Image number" refers to the randomized image orientation/number
+                for (Recognition recognition : updatedRecognitions) {
+                    double col = (recognition.getLeft() + recognition.getRight()) / 2 ;
+                    double row = (recognition.getTop()  + recognition.getBottom()) / 2 ;
+                    double width  = Math.abs(recognition.getRight() - recognition.getLeft()) ;
+                    double height = Math.abs(recognition.getTop()  - recognition.getBottom()) ;
+
+                    engine.telemetry.addData(""," ");
+                    engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 );
+                    engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col);
+                    engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height);
+                }
+            }
+        }
+    }
+
+    @Override
+    public void stop() {
         setHasFinished(true);
+
+        robot.getTfod().deactivate();
     }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java

@@ -128,6 +128,12 @@ public class DrivetrainDriverControl extends CyberarmState {
         // DEBUG: Toggle hardware fault
         if (button.equals("guide")) {
             robot.hardwareFault = !robot.hardwareFault;
+
+            if (robot.hardwareFault) {
+                robot.hardwareFaultMessage = "Manually triggered.";
+            } else {
+                robot.hardwareFaultMessage = "";
+            }
         }
 
         if (button.equals("back")) {