Make FtcRobotControllerActivity startup TACNET server since the ControlHub appears to block ON_BOOT_COMPLETED receivers...

(:

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java

@@ -49,6 +49,8 @@ import android.preference.PreferenceManager;
 import androidx.annotation.NonNull;
 import androidx.annotation.Nullable;
 import androidx.annotation.StringRes;
+import androidx.core.content.ContextCompat;
+
 import android.view.Menu;
 import android.view.MenuItem;
 import android.view.MotionEvent;
@@ -410,6 +412,12 @@ public class FtcRobotControllerActivity extends Activity
     checkPreferredChannel();
 
     AnnotatedHooksClassFilter.getInstance().callOnCreateMethods(this);
+
+    /* REV CONTROL HUB APPEARS TO PREVENT ANY APP NOT ON ITS WHITELIST FROM USING ON_BOOT_COMPLETED RECEIVERS */
+    /* USING THIS HACK DUE TO THAT...                                                                         */
+    Intent tacnetIntent = new Intent("org.timecrafters.TimeCraftersConfigurationTool.tacnet.ACTION_START_SERVER");
+    tacnetIntent.setPackage("org.timecrafters.TimeCraftersConfigurationTool");
+    ContextCompat.startForegroundService(context, tacnetIntent);
   }
 
   protected UpdateUI createUpdateUI() {

TeamCode/src/main/java/org/cyberarm/container/InputChecker.java

@@ -1,58 +0,0 @@
-package org.cyberarm.container;
-
-import android.util.Log;
-
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import java.lang.reflect.Field;
-import java.util.ArrayList;
-import java.util.HashMap;
-
-public class InputChecker {
-  private Gamepad gamepad;
-  private HashMap<String, Byte> buttons;
-  private ArrayList<String> buttonList;
-  private byte NULL    = 0,
-               PRESSED = 1,
-               RELEASED= 2;
-  public InputChecker(Gamepad gamepad) {
-    this.gamepad = gamepad;
-    buttons = new HashMap<>();
-    buttonList = new ArrayList<>();
-
-    buttonList.add("a"); buttonList.add("b"); buttonList.add("x"); buttonList.add("y");
-    buttonList.add("start"); buttonList.add("guide"); buttonList.add("back");
-    buttonList.add("left_bumper"); buttonList.add("right_bumper");
-    buttonList.add("left_stick_button"); buttonList.add("right_stick_button");
-    buttonList.add("dpad_left"); buttonList.add("dpad_right");
-    buttonList.add("dpad_up"); buttonList.add("dpad_down");
-  }
-
-  public void update() {
-    for (int i = 0; i < buttonList.size(); i++) {
-      try {
-        Field field = gamepad.getClass().getDeclaredField(buttonList.get(i));
-
-        if (field.getBoolean(gamepad)) {
-          buttons.put(buttonList.get(i), PRESSED);
-        } else {
-          if (buttons.get(buttonList.get(i)) != null && buttons.get(buttonList.get(i)) == PRESSED) {
-            buttons.put(buttonList.get(i), RELEASED);
-          }
-        }
-      } catch (NoSuchFieldException|IllegalAccessException e) {
-        e.printStackTrace();
-      }
-    }
-  }
-
-  public boolean check(String button) {
-    boolean state = false;
-    if (buttons.containsKey(button) && buttons.get(button) == RELEASED) {
-      Log.d("InputChecker","button \""+button+"\" has been released on \"gamepad"+gamepad.getGamepadId()+"\"");
-      state = true;
-      buttons.put(button, NULL);
-    }
-    return state;
-  }
-}

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

@@ -3,6 +3,7 @@ package org.cyberarm.engine.V2;
 import android.util.Log;
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.Gamepad;
 
 import java.util.concurrent.CopyOnWriteArrayList;
 
@@ -15,13 +16,15 @@ public abstract class CyberarmEngine extends OpMode {
 
   public static CyberarmEngine instance;
   //Array To Hold States
-  private CopyOnWriteArrayList<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
+  final private CopyOnWriteArrayList<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
   private int activeStateIndex = 0;
   private boolean isRunning;
 
-  private static String TAG = "PROGRAM.ENGINE";
+  private final static String TAG = "PROGRAM.ENGINE";
   public boolean showStateChildrenListInTelemetry = false;
 
+  private GamepadChecker gamepadCheckerGamepad1, gamepadCheckerGamepad2;
+
   /**
    * Called when INIT button on Driver Station is pushed
    * ENSURE to call super.init() if you override this method
@@ -29,6 +32,8 @@ public abstract class CyberarmEngine extends OpMode {
   public void init() {
     CyberarmEngine.instance = this;
     isRunning = false;
+    gamepadCheckerGamepad1 = new GamepadChecker(this, gamepad1);
+    gamepadCheckerGamepad2 = new GamepadChecker(this, gamepad2);
 
     setup();
 
@@ -43,12 +48,12 @@ public abstract class CyberarmEngine extends OpMode {
    * Setup states for engine to use
    * For example:
    * <pre>
-   * @<code>
+   * {@code
    *   public void setup() {
    *     addState(new TestState());
    *     addState(new AnotherState(100, 500));
    *   }
-   * </code>
+   * }
    * </pre>
    */
   public abstract void setup();
@@ -102,6 +107,9 @@ public abstract class CyberarmEngine extends OpMode {
     } else {
       stateTelemetry(state);
     }
+
+    gamepadCheckerGamepad1.update();
+    gamepadCheckerGamepad2.update();
   }
 
   /**
@@ -167,14 +175,11 @@ public abstract class CyberarmEngine extends OpMode {
     final CyberarmState finalState = state;
 //    if (state.isRunning()) { return; } // Assume that we have already started running this state
 
-    new Thread(new Runnable() {
-      @Override
-      public void run() {
-        finalState.prestart();
-        finalState.start();
-        finalState.startTime = System.currentTimeMillis();
-        finalState.run();
-      }
+    new Thread(() -> {
+      finalState.prestart();
+      finalState.start();
+      finalState.startTime = System.currentTimeMillis();
+      finalState.run();
     }).start();
 
     for (CyberarmState kid : state.children) {
@@ -199,7 +204,7 @@ public abstract class CyberarmEngine extends OpMode {
    * Inserts state after the query state plus an offset to ensure logical insertion
    * @param query State to add state after
    * @param state State to be inserted
-   * @return
+   * @return CyberarmState
    */
   public CyberarmState insertState(CyberarmState query, CyberarmState state) {
     int index = cyberarmStates.indexOf(query) + query.insertOffset;
@@ -213,6 +218,48 @@ public abstract class CyberarmEngine extends OpMode {
     return state;
   }
 
+  private void buttonDownForStates(CyberarmState state, Gamepad gamepad, String button) {
+    state.buttonDown(gamepad, button);
+
+    for (CyberarmState child : state.children) {
+      child.buttonDown(gamepad, button);
+    }
+  }
+
+  private void buttonUpForStates(CyberarmState state, Gamepad gamepad, String button) {
+    state.buttonUp(gamepad, button);
+
+    for (CyberarmState child : state.children) {
+      child.buttonUp(gamepad, button);
+    }
+  }
+
+  /**
+   * Called by GamepadChecker when it detects that a gamepad button has been pressed
+   * @param gamepad Gamepad
+   * @param button String
+   */
+  protected void buttonDown(Gamepad gamepad, String button) {
+    try {
+      buttonDownForStates(cyberarmStates.get(activeStateIndex), gamepad, button);
+    } catch(IndexOutOfBoundsException e){
+      /* loop will handle this in a few milliseconds */
+    }
+  }
+
+  /**
+   * Called by GamepadChecker when it detects that a gamepad button has been released
+   * @param gamepad Gamepad
+   * @param button String
+   */
+  protected void buttonUp(Gamepad gamepad, String button) {
+    try {
+      buttonUpForStates(cyberarmStates.get(activeStateIndex), gamepad, button);
+    } catch(IndexOutOfBoundsException e){
+      /* loop will handle this in a few milliseconds */
+    }
+  }
+
   /**
    * This will return false while Engine.setup() is executing, and be true after.
    * @return Whether the engine main loop is running

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

@@ -2,6 +2,8 @@ package org.cyberarm.engine.V2;
 
 import android.util.Log;
 
+import com.qualcomm.robotcore.hardware.Gamepad;
+
 import java.util.concurrent.CopyOnWriteArrayList;
 
 /**
@@ -65,6 +67,23 @@ public abstract class CyberarmState implements Runnable {
   public void stop() {
   }
 
+  /**
+   * Called when GamepadChecker detects that a gamepad button has been pressed
+   * @param gamepad Gamepad
+   * @param button String
+   */
+  public void buttonDown(Gamepad gamepad, String button) {
+  }
+
+
+  /**
+   * Called when GamepadChecker detects that a gamepad button has been released
+   * @param gamepad Gamepad
+   * @param button String
+   */
+  public void buttonUp(Gamepad gamepad, String button) {
+  }
+
   /**
    * Add a state which runs in parallel with this one
    */
@@ -139,7 +158,7 @@ public abstract class CyberarmState implements Runnable {
 
   /**
    * Set whether state has finished or not
-   * @param value
+   * @param value boolean
    */
   public void setHasFinished(boolean value) {
     hasFinished = value;

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

@@ -0,0 +1,61 @@
+package org.cyberarm.engine.V2;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import java.lang.reflect.Field;
+import java.util.HashMap;
+
+public class GamepadChecker {
+  private final CyberarmEngine engine;
+  private final Gamepad gamepad;
+  private final HashMap<String, Boolean> buttons = new HashMap<>();
+
+  public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) {
+    this.engine = engine;
+    this.gamepad = gamepad;
+
+    buttons.put("a", false);
+    buttons.put("b", false);
+    buttons.put("x", false);
+    buttons.put("y", false);
+
+    buttons.put("start", false);
+    buttons.put("guide", false);
+    buttons.put("back", false);
+
+    buttons.put("left_bumper", false);
+    buttons.put("right_bumper", false);
+
+    buttons.put("left_stick_button", false);
+    buttons.put("right_stick_button", false);
+
+    buttons.put("dpad_left", false);
+    buttons.put("dpad_right", false);
+    buttons.put("dpad_up", false);
+    buttons.put("dpad_down", false);
+  }
+
+  public void update() {
+    for (String btn : buttons.keySet()) {
+      try {
+        Field field = gamepad.getClass().getDeclaredField(btn);
+
+        if (field.getBoolean(gamepad)) {
+          if (!buttons.get(btn)) {
+            engine.buttonDown(gamepad, btn);
+          }
+
+          buttons.put(btn, true);
+        } else {
+          if (buttons.get(btn)) {
+            engine.buttonUp(gamepad, btn);
+          }
+
+          buttons.put(btn, false);
+        }
+      } catch (NoSuchFieldException|IllegalAccessException e) {
+        e.printStackTrace();
+      }
+    }
+  }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java

@@ -0,0 +1,58 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.CollectorToggle;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TensorFlowState;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmRiser;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+@Autonomous (name = "Blue WareHouse", group = "blue")
+public class BlueWarehouseEngine extends CyberarmEngine {
+    Robot robot;
+
+    @Override
+    public void setup() {
+        this.robot = new Robot(this);
+
+        addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "BlueWarehouseAutonomous", "01_0"));
+        addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "01_1"));
+//        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "02_0"));
+//        addState(new TurretArmRiser(robot, robot.whiteArmRiser, "BlueWarehouseAutonomous", "03_0_middle"));
+//        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "04_0_middle"));
+        addState(new CollectorToggle(robot, robot.collectorWhite, "BlueWarehouseAutonomous", "05_0"));
+        addState(new CollectorToggle(robot, robot.collectorWhite, "BlueWarehouseAutonomous", "06_0"));
+        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "07_0"));
+        addState(new DriveState(robot,"BlueWarehouseAutonomous", "08_0"));
+        addState(new DriveState(robot, "BlueWarehouseAutonomous", "09_0"));
+        addState(new DriveState(robot, "BlueWarehouseAutonomous", "10_0"));
+
+    }
+
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition());
+        telemetry.addData("White Riser Arm", robot.whiteArmRiser.getCurrentPosition());
+        telemetry.addData("white Turret Switch", robot.whiteMag.isPressed());
+        telemetry.addData("white Turret orbit Power", robot.turretServoWhite.getPower());
+        telemetry.addData("White Door Position", robot.whiteDispenser.getPosition());
+        telemetry.addLine();
+
+        telemetry.addData("Orange Riser Arm", robot.orangeArmRiser.getCurrentPosition());
+        telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition());
+        telemetry.addData("orange Turret Switch", robot.orangeMag.isPressed());
+        telemetry.addData("orange Turret Orbit Power", robot.turretServoOrange.getPower());
+        telemetry.addData("Orange Door Position", robot.orangeDispenser.getPosition());
+        telemetry.addLine();
+
+        telemetry.addData("driveWarehouseLeft", robot.driveWarehouseLeft.getCurrentPosition());
+        telemetry.addData("driveWarehouseRight", robot.driveWarehouseRight.getCurrentPosition());
+        telemetry.addData("driveGoalLeft", robot.driveGoalLeft.getCurrentPosition());
+        telemetry.addData("driveGoalRight", robot.driveGoalRight.getCurrentPosition());
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java

@@ -0,0 +1,59 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.CollectorToggle;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DepositorDoor;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TensorFlowState;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmRiser;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+@Autonomous (name = "Red WareHouse", group = "red")
+public class RedWarehouseEngine extends CyberarmEngine {
+    Robot robot;
+
+    @Override
+    public void setup() {
+        this.robot = new Robot(this);
+
+        addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "RedWarehouseAutonomous", "01_0"));
+        addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "RedWarehouseAutonomous", "01_1"));
+//        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "02_0"));
+//        addState(new TurretArmRiser(robot, robot.whiteArmRiser, "RedWarehouseAutonomous", "03_0_middle"));
+//        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "04_0"));
+        addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "05_0"));
+        addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "06_0"));
+        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "07_0"));
+        addState(new DriveState(robot,"RedWarehouseAutonomous", "08_0"));
+        addState(new DriveState(robot, "RedWarehouseAutonomous", "09_0"));
+        addState(new TurretOrbit(robot, robot.turretServoWhite, null, "RedWarehouseAutonomous", "10_0"));
+        addState(new DriveState(robot, "RedWarehouseAutonomous", "10_1"));
+
+    }
+
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition());
+        telemetry.addData("White Riser Arm", robot.whiteArmRiser.getCurrentPosition());
+        telemetry.addData("white Turret Switch", robot.whiteMag.isPressed());
+        telemetry.addData("white Turret orbit Power", robot.turretServoWhite.getPower());
+        telemetry.addData("White Door Position", robot.whiteDispenser.getPosition());
+        telemetry.addLine();
+
+        telemetry.addData("Orange Riser Arm", robot.orangeArmRiser.getCurrentPosition());
+        telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition());
+        telemetry.addData("orange Turret Switch", robot.orangeMag.isPressed());
+        telemetry.addData("orange Turret Orbit Power", robot.turretServoOrange.getPower());
+        telemetry.addData("Orange Door Position", robot.orangeDispenser.getPosition());
+        telemetry.addLine();
+
+        telemetry.addData("driveWarehouseLeft", robot.driveWarehouseLeft.getCurrentPosition());
+        telemetry.addData("driveWarehouseRight", robot.driveWarehouseRight.getCurrentPosition());
+        telemetry.addData("driveGoalLeft", robot.driveGoalLeft.getCurrentPosition());
+        telemetry.addData("driveGoalRight", robot.driveGoalRight.getCurrentPosition());
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java

@@ -0,0 +1,41 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class CollectorToggle extends CyberarmState {
+    final static public int MODE_REVERSE = -1;
+    final static public int MODE_COLLECT = 1;
+    final static public int MODE_STOPPED = 0;
+    double time;
+    CRServo servo;
+    int power;
+
+    public CollectorToggle(Robot robot, CRServo servo, String groupName, String actionName) {
+        this.servo = servo;
+        this.power = robot.configuration.variable(groupName, actionName, "power").value();
+        this.time = robot.configuration.variable(groupName, actionName, "time").value();
+
+    }
+
+    @Override
+    public void exec() {
+        servo.setPower(power);
+
+        if (runTime() >= time){
+
+            servo.setPower(0);
+
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("runtime", runTime());
+        engine.telemetry.addData("time", time);
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DepositorDoor.java

@@ -0,0 +1,30 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class DepositorDoor extends CyberarmState {
+
+    Servo servo;
+    double targetPosition;
+    long time;
+
+    public DepositorDoor(Robot robot, Servo servo, String groupName, String actionName) {
+        this.servo = servo;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.time = robot.configuration.variable(groupName, actionName, "time").value();
+    }
+
+    @Override
+    public void exec() {
+        if (runTime() < time) {
+            servo.setPosition(targetPosition);
+        }
+        else {
+            setHasFinished(true);
+        }
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DriveState.java

@@ -0,0 +1,61 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class DriveState extends CyberarmState {
+    Robot robot;
+    int distanceLeft, distanceRight;
+    double powerLeft, powerRight;
+
+    public DriveState(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.distanceLeft = robot.configuration.variable(groupName, actionName, "distanceLeft").value();
+        this.distanceRight = robot.configuration.variable(groupName, actionName, "distanceRight").value();
+        this.powerLeft = robot.configuration.variable(groupName, actionName, "powerLeft").value();
+        this.powerRight = robot.configuration.variable(groupName, actionName, "powerRight").value();
+    }
+
+    @Override
+    public void start() {
+        robot.driveGoalLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveGoalRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveWarehouseLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveWarehouseRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        robot.driveGoalLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveGoalRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveWarehouseLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveWarehouseRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        if (Math.abs(robot.driveGoalLeft.getCurrentPosition()) >= distanceLeft &&
+                Math.abs(robot.driveGoalRight.getCurrentPosition()) >= distanceRight) {
+            robot.driveGoalLeft.setPower(0);
+            robot.driveGoalRight.setPower(0);
+            robot.driveWarehouseRight.setPower(0);
+            robot.driveWarehouseLeft.setPower(0);
+            setHasFinished(true);
+        } else {
+            robot.driveGoalLeft.setPower(powerLeft);
+            robot.driveGoalRight.setPower(powerRight);
+            robot.driveWarehouseRight.setPower(powerRight);
+            robot.driveWarehouseLeft.setPower(powerLeft);
+
+        }
+
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("distance left", distanceLeft);
+        engine.telemetry.addData("distance right", distanceRight);
+        engine.telemetry.addData("power left", powerLeft);
+        engine.telemetry.addData("power Right", powerRight);
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java

@@ -0,0 +1,57 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class FaceState extends CyberarmState {
+    private double faceDirection, tolerance, power;
+    private Robot robot;
+    public FaceState(Robot robot, double faceDirection, double tolerance, double power) {
+        this.robot = robot;
+        this.faceDirection = faceDirection;
+        this.tolerance = tolerance;
+        this.power = power;
+    }
+
+    @Override
+    public void start() {
+        robot.driveGoalLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveGoalRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveWarehouseLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveWarehouseRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        robot.driveGoalLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveGoalRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveWarehouseLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveWarehouseRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        robot.updateRobotOrientation();
+
+        if (robot.heading() - tolerance > faceDirection) {
+            robot.driveGoalLeft.setPower(power);
+            robot.driveWarehouseLeft.setPower(power);
+            robot.driveGoalRight.setPower(0);
+            robot.driveWarehouseRight.setPower(0);
+        }
+
+        else if (robot.heading() + tolerance < faceDirection){
+            robot.driveGoalLeft.setPower(0);
+            robot.driveWarehouseLeft.setPower(0);
+            robot.driveGoalRight.setPower(power);
+            robot.driveWarehouseRight.setPower(power);
+        }
+
+        else {
+            robot.driveGoalLeft.setPower(0);
+            robot.driveWarehouseLeft.setPower(0);
+            robot.driveGoalRight.setPower(0);
+            robot.driveWarehouseRight.setPower(0);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java

@@ -0,0 +1,86 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+import java.util.List;
+
+public class TensorFlowState extends CyberarmState {
+
+    Robot robot;
+    private List<Recognition> recognitions;
+    private double checkTime;
+    private int manualPath;
+    private int path = 0;
+
+    private String groupName;
+    private DcMotor armRiser, armExtension;
+
+    public TensorFlowState(Robot robot, DcMotor armRiser, DcMotor armExtension, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.armRiser = armRiser;
+        this.armExtension = armExtension;
+    }
+
+    @Override
+    public void start() {
+        robot.activateVuforia();
+        robot.activateTensorflow();
+    }
+
+    @Override
+    public void exec() {
+        recognitions = robot.tensorflowDetections();
+//
+        if (runTime() < checkTime) {
+            if (manualPath != -1) {
+
+                if (recognitions != null) {
+                    if (recognitions.size() == 1) {
+                        Recognition recognition = recognitions.get(0);
+
+                        if (recognition.getLabel().equals("duck")){
+                            if (recognition.getLeft() < 320) {
+                                path = 0;
+                            } else {
+                                path = 1;
+                            }
+                        }
+                    } else {
+                        path = 2;
+                    }
+                }
+            }
+        } else {
+            if (path == 0){
+                addState(new TurretArmExtension(robot, armExtension, groupName, "02_0"));
+                addState(new TurretArmRiser(robot, armRiser, groupName, "03_0_bottom"));
+                addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_bottom"));
+            } else if (path == 1){
+                addState(new TurretArmExtension(robot, armExtension, groupName, "02_0"));
+                addState(new TurretArmRiser(robot, armRiser, groupName, "03_0_middle"));
+                addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_middle"));
+            } else {
+                addState(new TurretArmExtension(robot, armExtension, groupName, "02_0"));
+                addState(new TurretArmRiser(robot, armRiser, groupName, "03_0_top"));
+                addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_top"));
+            }
+
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        for (Recognition recognition : robot.tensorflowDetections()) {
+            engine.telemetry.addData("Label", recognition.getLabel());
+            engine.telemetry.addData("Left", recognition.getLeft());
+            engine.telemetry.addData("Top", recognition.getTop());
+            engine.telemetry.addData("Confidence", recognition.getConfidence());
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java

@@ -0,0 +1,43 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class TurretArmExtension extends CyberarmState {
+
+    private Robot robot;
+    private DcMotor motor;
+    private int targetPosition, tolerance;
+    private double power;
+
+    public TurretArmExtension(Robot robot, DcMotor motor, String groupName, String actionName) {
+        this.robot = robot;
+        this.motor = motor;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.power = robot.configuration.variable(groupName, actionName, "power").value();
+        this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
+    }
+
+    @Override
+    public void start() {
+        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        if (motor.getCurrentPosition() < targetPosition - tolerance){
+            motor.setPower(power);
+        }
+
+        else if (motor.getCurrentPosition() > targetPosition + tolerance){
+            motor.setPower(-power);
+        }
+        else {
+            setHasFinished(true);
+            motor.setPower(0);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java

@@ -0,0 +1,43 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class TurretArmRiser extends CyberarmState {
+
+    private Robot robot;
+    private DcMotor motor;
+    private int targetPosition, tolerance;
+    private double power;
+
+    public TurretArmRiser(Robot robot, DcMotor motor, String groupName, String actionName) {
+        this.robot = robot;
+        this.motor = motor;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.power = robot.configuration.variable(groupName, actionName, "power").value();
+        this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
+    }
+
+    @Override
+    public void start() {
+        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        if (motor.getCurrentPosition() < targetPosition - tolerance){
+            motor.setPower(power);
+        }
+
+        else if (motor.getCurrentPosition() > targetPosition + tolerance){
+            motor.setPower(-power);
+        }
+        else {
+            setHasFinished(true);
+            motor.setPower(0);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java

@@ -0,0 +1,35 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.hardware.rev.RevTouchSensor;
+import com.qualcomm.robotcore.hardware.CRServo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+
+public class TurretOrbit extends CyberarmState {
+    private Robot robot;
+    private CRServo servo;
+    private double time;
+    private double power;
+    private RevTouchSensor magnetSwitch;
+    public TurretOrbit(Robot robot, CRServo servo, RevTouchSensor magnetSwitch, String groupName, String actionName) {
+        this.robot = robot;
+        this.servo = servo;
+        this.time = robot.configuration.variable(groupName, actionName, "time").value();
+        this. power = robot.configuration.variable(groupName, actionName, "power").value();
+        this. magnetSwitch = magnetSwitch;
+    }
+
+    @Override
+    public void exec() {
+  if (magnetSwitch.isPressed() || runTime() < time ){
+      servo.setPower(0);
+      setHasFinished(true);
+  }
+  else {
+      servo.setPower(power);
+  }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java

@@ -1,41 +1,99 @@
 package org.timecrafters.FreightFrenzy.Competition.Common;
 
+import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
+
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.hardware.rev.RevTouchSensor;
+import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.Servo;
 
 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.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
+import java.util.ArrayList;
+import java.util.List;
+
 public class Robot {
+    private static final float mmPerInch        = 25.4f;
+    private static final float mmTargetHeight   = 6 * mmPerInch;  // the height of the center of the target image above the floor
+    private static final float halfField        = 72 * mmPerInch;
+    private static final float halfTile         = 12 * mmPerInch;
+    private static final float oneAndHalfTile   = 36 * mmPerInch;
+
+    private static final String TENSORFLOW_MODEL_ASSET = "FreightFrenzy_BCDM.tflite";
+    private static final String[] TENSORFLOW_MODEL_LABELS = {
+            "Ball",
+            "Cube",
+            "Duck",
+            "Marker"
+    };
+    private static final String VUFORIA_KEY = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
+    private static final float VUFORIA_CAMERA_FORWARD_DISPLACEMENT  = 0.0f * mmPerInch; // Camera lens forward distance from robot center
+    private static final float VUFORIA_CAMERA_VERTICAL_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens height above ground
+    private static final float VUFORIA_CAMERA_LEFT_DISPLACEMENT     = 0.0f * mmPerInch; // Camera lens left distance from robot center
+
     private final CyberarmEngine engine;
     public TimeCraftersConfiguration configuration;
 
     // Shiny
     public RevBlinkinLedDriver leds;
+    public TFObjectDetector tensorflow;
+    private List<Recognition> tensorflowRecognitions = new ArrayList<>();
+    public VuforiaLocalizer vuforia;
+    private VuforiaTrackables vuforiaTargets;
+    private List<VuforiaTrackable> vuforiaTrackables;
+    private OpenGLMatrix vuforiaCameraLocationOnRobot = OpenGLMatrix
+            .translation(VUFORIA_CAMERA_FORWARD_DISPLACEMENT, VUFORIA_CAMERA_LEFT_DISPLACEMENT, VUFORIA_CAMERA_VERTICAL_DISPLACEMENT)
+            .multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XZY, AngleUnit.DEGREES, 90, 90, 0));
+    private RobotLocation vuforiaLastLocation;
+    private Orientation orientation;
 
     // Sensors
     public BNO055IMU imu;
+    public RevTouchSensor whiteMag;
+    public RevTouchSensor orangeMag;
 
     // Drivetrain
     public static final double WHEEL_CIRCUMFERENCE = Math.PI * 8;
     public static final int COUNTS_PER_REVOLUTION = 1000;
 
-    public DcMotor driveFrontLeft, driveFrontRight, driveBackLeft, driveBackRight;
+    public DcMotor driveWarehouseLeft, driveWarehouseRight, driveGoalLeft, driveGoalRight,
+            orangeArmRiser, orangeArmBobbin, whiteArmRiser, whiteArmBobbin;
 
     // Collector
-    public DcMotor collectorBobbin;
-    public Servo collectorDoor;
+    public Servo orangeDispenser;
+    public CRServo turretServoOrange;
 
     // Depositor
-    public DcMotor depositorBobbin;
-    public Servo depositorDoor;
+    public Servo whiteDispenser;
+    // Depositor, and carousel
+    public CRServo turretServoWhite, carouselWheel, collectorOrange, collectorWhite;
+
+
 
     public Robot(CyberarmEngine engine) {
         this.engine = engine;
-
+        initMagnetSwitches();
         initConfiguration();
         initBlinkin();
         initIMU();
@@ -43,18 +101,79 @@ public class Robot {
         initCollector();
         initDepositor();
         initCarousel();
+        initVuforia();
+        initTensorflow();
     }
 
-    public double rotation() {
-        return imu.getAngularOrientation().firstAngle;
+    private void initMagnetSwitches() {
+        whiteMag = engine.hardwareMap.get(RevTouchSensor.class, "whiteMag");
+        orangeMag = engine.hardwareMap.get(RevTouchSensor.class, "orangeMag");
     }
 
-    public double pitch() {
-        return imu.getAngularOrientation().secondAngle;
+    public double heading() {
+        return orientation.firstAngle;
     }
 
     public double roll() {
-        return imu.getAngularOrientation().thirdAngle;
+        return orientation.secondAngle;
+    }
+
+    public double pitch() {
+        return orientation.thirdAngle;
+    }
+
+    public void updateRobotOrientation(){
+        orientation = imu.getAngularOrientation();
+    }
+
+    public void activateTensorflow() {
+        tensorflow.activate();
+    }
+
+    public List<Recognition> tensorflowDetections() {
+        List<Recognition> updateRecognitions = tensorflow.getUpdatedRecognitions();
+
+        if (updateRecognitions != null) {
+            tensorflowRecognitions = updateRecognitions;
+        }
+
+        return tensorflowRecognitions;
+    }
+
+    public void deactivateTensorflow() {
+        tensorflow.deactivate();
+    }
+
+    public void activateVuforia() {
+        vuforiaTargets.activate();
+    }
+
+    public RobotLocation vuforiaLocation() {
+        for (VuforiaTrackable trackable : vuforiaTrackables) {
+            if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
+                OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
+                if (robotLocationTransform != null) {
+                    vuforiaLastLocation = new RobotLocation(robotLocationTransform);
+                }
+
+                // Recycle old position since there is no new data
+                return vuforiaLastLocation;
+            }
+        }
+
+        // Return null if there is no visible trackable
+        return null;
+    }
+
+    private void vuforiaIdentifyTarget(int targetIndex, String targetName, float dx, float dy, float dz, float rx, float ry, float rz) {
+        VuforiaTrackable aTarget = vuforiaTargets.get(targetIndex);
+        aTarget.setName(targetName);
+        aTarget.setLocation(OpenGLMatrix.translation(dx, dy, dz)
+                .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, rx, ry, rz)));
+    }
+
+    public void deactivateVuforia() {
+        vuforiaTargets.deactivate();
     }
 
     public double ticksToInches(int ticks) {
@@ -83,27 +202,88 @@ public class Robot {
 
         this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
         imu.initialize(parameters);
+
+        imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
     }
 
     private void initDrivetrain() {
-        driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight");
-        driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft");
-        driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight");
-        driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft");
+        driveWarehouseRight = engine.hardwareMap.dcMotor.get("driveWarehouseRight");
+        driveWarehouseLeft  = engine.hardwareMap.dcMotor.get("driveWarehouseLeft");
+        driveGoalRight      = engine.hardwareMap.dcMotor.get("driveGoalRight");
+        driveGoalLeft       = engine.hardwareMap.dcMotor.get("driveGoalLeft");
 
-        driveFrontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
-        driveBackLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        driveWarehouseLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveWarehouseRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        driveGoalLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveGoalRight.setDirection(DcMotorSimple.Direction.REVERSE);
     }
 
     private void initCollector() {
+        orangeArmBobbin = engine.hardwareMap.dcMotor.get("orangeArmBobbin");
+        orangeDispenser = engine.hardwareMap.servo.get("orangeDispenser");
+        orangeArmBobbin.setDirection(DcMotorSimple.Direction.REVERSE);
+        turretServoOrange = engine.hardwareMap.crservo.get("turretServoOrange");
+        orangeArmRiser = engine.hardwareMap.dcMotor.get("orangeArmRiser");
+        orangeArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
+        orangeArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        collectorOrange = engine.hardwareMap.crservo.get("collectorOrange");
 
+        orangeDispenser.setPosition(0.5);
     }
 
     private void initDepositor(){
-
+        whiteDispenser = engine.hardwareMap.servo.get("whiteDispenser");
+        whiteArmBobbin = engine.hardwareMap.dcMotor.get("whiteArmBobbin");
+        whiteArmBobbin.setDirection(DcMotorSimple.Direction.REVERSE);
+        turretServoWhite = engine.hardwareMap.crservo.get("turretServoWhite");
+        whiteArmRiser = engine.hardwareMap.dcMotor.get("whiteArmRiser");
+        whiteArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
+        whiteArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        collectorWhite = engine.hardwareMap.crservo.get("collectorWhite");
     }
 
     private void initCarousel() {
+        carouselWheel = engine.hardwareMap.crservo.get("carouselWheel");
+    }
 
+    private void initVuforia() {
+        int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+
+        parameters.vuforiaLicenseKey = VUFORIA_KEY;
+        parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
+        parameters.useExtendedTracking = false;
+
+        vuforia = ClassFactory.getInstance().createVuforia(parameters);
+
+        vuforiaTargets = vuforia.loadTrackablesFromAsset("FreightFrenzy");
+
+        vuforiaTrackables = new ArrayList<>(vuforiaTargets);
+
+        // Name and locate each trackable object
+        vuforiaIdentifyTarget(0, "Blue Storage",       -halfField,  oneAndHalfTile, mmTargetHeight, 90, 0, 90);
+        vuforiaIdentifyTarget(1, "Blue Alliance Wall",  halfTile,   halfField,      mmTargetHeight, 90, 0, 0);
+        vuforiaIdentifyTarget(2, "Red Storage",        -halfField, -oneAndHalfTile, mmTargetHeight, 90, 0, 90);
+        vuforiaIdentifyTarget(3, "Red Alliance Wall",   halfTile,  -halfField,      mmTargetHeight, 90, 0, 180);
+
+        for (VuforiaTrackable trackable : vuforiaTrackables) {
+            ((VuforiaTrackableDefaultListener) trackable.getListener())
+                    .setCameraLocationOnRobot(parameters.cameraName, vuforiaCameraLocationOnRobot);
+        }
+    }
+
+    private void initTensorflow() {
+        int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+
+        TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        parameters.minResultConfidence = 0.8f;
+        parameters.isModelTensorFlow2 = true;
+        parameters.inputSize = 320;
+
+        tensorflow = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
+        tensorflow.loadModelFromAsset(TENSORFLOW_MODEL_ASSET, TENSORFLOW_MODEL_LABELS);
     }
 }

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/RobotLocation.java

@@ -0,0 +1,25 @@
+package org.timecrafters.FreightFrenzy.Competition.Common;
+
+import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+
+public class RobotLocation {
+    public double x, y, z, roll, pitch, heading;
+    public RobotLocation(OpenGLMatrix matrix) {
+        VectorF translation = matrix.getTranslation();
+        Orientation rotation = Orientation.getOrientation(
+                matrix, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
+
+        x = translation.get(0);
+        y = translation.get(1);
+        z = translation.get(2);
+
+        roll = rotation.firstAngle;
+        pitch = rotation.secondAngle;
+        heading = rotation.thirdAngle;
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/Engines/TeleOpEngine.java

@@ -0,0 +1,15 @@
+package org.timecrafters.FreightFrenzy.Competition.TeleOp.Engines;
+//adb connect 192.168.43.1
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.Competition.TeleOp.States.TeleOpState;
+
+@TeleOp(name = "TeleOp")
+public class TeleOpEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new TeleOpState(new Robot(this)));
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java

@@ -0,0 +1,281 @@
+package org.timecrafters.FreightFrenzy.Competition.TeleOp.States;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class TeleOpState extends CyberarmState {
+    Robot robot;
+    double maxDriveSpeed, maxCollectorArmSpeed, maxDepositorArmSpeed;
+    int maxArmTravelDistance, minArmTravelDistance;
+    boolean armLimitToggle = false, orangeCollectorToggle = false, whiteCollectorToggle = false;
+    boolean allianceRedDriver1 = true, allianceRedDriver2 = true;
+
+    public TeleOpState(Robot robot) {
+        this.robot = robot;
+     maxDriveSpeed = 1;
+     maxCollectorArmSpeed = 1;
+     maxDepositorArmSpeed = 1;
+     minArmTravelDistance = 0;
+     maxArmTravelDistance = 4000;
+
+    }
+
+
+    @Override
+    public void start() {
+        super.start();
+        // FIXME: Don't reset when doing autonomous stuff
+        robot.whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.orangeArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.orangeArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        // GamePad 1
+
+        // TankDrive
+        robot.driveWarehouseLeft.setPower(-engine.gamepad1.left_stick_y * maxDriveSpeed);
+        robot.driveGoalLeft.setPower(-engine.gamepad1.left_stick_y * maxDriveSpeed);
+
+        robot.driveWarehouseRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed);
+        robot.driveGoalRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed);
+
+        // dispenser powered
+        if (engine.gamepad1.b){
+            robot.orangeDispenser.setPosition(0);
+        }
+        // if not pressed dispenser off
+        else {
+            robot.orangeDispenser.setPosition(0.5);
+        }
+
+        //  if one of triggers pressed arm extends or unextends, there is also a limit on how far arm can extend
+        if (robot.orangeArmBobbin.getCurrentPosition() <= maxArmTravelDistance && engine.gamepad1.left_trigger > 0) {
+
+            robot.orangeArmBobbin.setPower(engine.gamepad1.left_trigger * maxDepositorArmSpeed);
+        } else {
+            if (engine.gamepad1.left_trigger <= 0 && robot.orangeArmBobbin.getCurrentPosition() >= minArmTravelDistance) {
+                robot.orangeArmBobbin.setPower(-engine.gamepad1.right_trigger * maxDepositorArmSpeed);
+            } else {
+                robot.orangeArmBobbin.setPower(0);
+            }
+
+        }
+
+        turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRedDriver1);
+
+        // if dpad verticles pressed arm rises or lowers...
+        if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) {
+
+            if (engine.gamepad1.dpad_up) {
+                robot.orangeArmRiser.setPower(0.8);
+            }
+
+            if (engine.gamepad1.dpad_down) {
+                robot.orangeArmRiser.setPower(-0.5);
+            }
+        }
+        // nothing pressed nothing move...
+        else {
+            robot.orangeArmRiser.setPower(0);
+        }
+
+        // button x pressed carousel wheel move.
+
+        if (engine.gamepad1.x){
+            robot.carouselWheel.setPower(1);
+        }
+
+        else {
+            robot.carouselWheel.setPower(0);
+        }
+
+        // GamePad 2
+
+        // if triggers are pressed then arm extends or unextends, there is also a limit on how far arm can extend
+        if (robot.whiteArmBobbin.getCurrentPosition() <= maxArmTravelDistance && engine.gamepad2.left_trigger > 0) {
+
+            robot.whiteArmBobbin.setPower(engine.gamepad2.left_trigger * maxDepositorArmSpeed);
+        } else {
+            if (engine.gamepad2.left_trigger <= 0 && robot.whiteArmBobbin.getCurrentPosition() >= minArmTravelDistance) {
+                robot.whiteArmBobbin.setPower(-engine.gamepad2.right_trigger * maxDepositorArmSpeed);
+            } else {
+                robot.whiteArmBobbin.setPower(0);
+            }
+
+        }
+
+
+
+        // if b is pressed then depositor door opens, if not pressed not opening.
+        if (engine.gamepad2.b){
+            robot.whiteDispenser.setPosition(.5);
+        }
+
+        else {
+            robot.whiteDispenser.setPosition(0);
+        }
+
+
+        turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRedDriver2);
+        // if dpad verticles pressed arm rises or lowers
+        if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) {
+
+            if (engine.gamepad2.dpad_up) {
+                robot.whiteArmRiser.setPower(8);
+            }
+
+            if (engine.gamepad2.dpad_down) {
+                robot.whiteArmRiser.setPower(-0.5);
+            }
+        }
+        // nothing pressed nothing move...
+        else {
+            robot.whiteArmRiser.setPower(0);
+        }
+    }
+
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("arm limit toggle", armLimitToggle);
+        engine.telemetry.addData("driver A turret inverted", allianceRedDriver1);
+        engine.telemetry.addData("driver B turret inverted", allianceRedDriver2);
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition());
+        engine.telemetry.addData("White Riser Arm", robot.whiteArmRiser.getCurrentPosition());
+        engine.telemetry.addData("white collector toggle ", whiteCollectorToggle);
+        engine.telemetry.addData("white Turret Switch", robot.whiteMag.isPressed());
+        engine.telemetry.addData("white Turret orbit Power", robot.turretServoWhite.getPower());
+        engine.telemetry.addData("White Door Position", robot.whiteDispenser.getPosition());
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("Orange Riser Arm", robot.orangeArmRiser.getCurrentPosition());
+        engine.telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition());
+        engine.telemetry.addData("orange collector toggle ", orangeCollectorToggle);
+        engine.telemetry.addData("orange Turret Switch", robot.orangeMag.isPressed());
+        engine.telemetry.addData("orange Turret Orbit Power", robot.turretServoOrange.getPower());
+        engine.telemetry.addData("Orange Door Position", robot.orangeDispenser.getPosition());
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("driveWarehouseLeft", robot.driveWarehouseLeft.getCurrentPosition());
+        engine.telemetry.addData("driveWarehouseRight", robot.driveWarehouseRight.getCurrentPosition());
+        engine.telemetry.addData("driveGoalLeft", robot.driveGoalLeft.getCurrentPosition());
+        engine.telemetry.addData("driveGoalRight", robot.driveGoalRight.getCurrentPosition());
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        // this is a toggle of a limit for arm extension in the case of the robot crashes
+        // in the middle of the game we can bring the arm back in and extend and reset its position.
+        System.out.println("GamePad: " + gamepad.getGamepadId() + " Button: " + button);
+
+        if (button.equals("y")) {
+            System.out.println("Y has been pressed! (" + armLimitToggle + ")");
+
+            if (!armLimitToggle) {
+                armLimitToggle = true;
+                minArmTravelDistance = -8500;
+                maxArmTravelDistance = 8500;
+            } else {
+                armLimitToggle = false;
+                minArmTravelDistance = 0;
+                maxArmTravelDistance = 4000;
+                robot.orangeArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                robot.whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                robot.orangeArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                robot.whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            }
+        }
+
+        if (gamepad == engine.gamepad1) {
+            switch (button) {
+                case "left_bumper":
+                    if (orangeCollectorToggle) {
+                        orangeCollectorToggle = false;
+                        robot.collectorOrange.setPower(0);
+                    } else {
+                        orangeCollectorToggle = true;
+                        robot.collectorOrange.setPower(1);
+                    }
+                    break;
+                case "right_bumper":
+                    if (orangeCollectorToggle) {
+                        orangeCollectorToggle = false;
+                        robot.collectorOrange.setPower(0);
+                    } else {
+                        orangeCollectorToggle = true;
+                        robot.collectorOrange.setPower(-1);
+                    }
+                    break;
+
+                case "guide":
+                    allianceRedDriver1 = !allianceRedDriver1;
+                    break;
+
+
+            }
+        }
+
+        if (gamepad == engine.gamepad2) {
+            switch (button) {
+
+                case "left_bumper":
+                    if (whiteCollectorToggle) {
+                        whiteCollectorToggle = false;
+                        robot.collectorWhite.setPower(0);
+                    } else {
+                        whiteCollectorToggle = true;
+                        robot.collectorWhite.setPower(-1);
+                    }
+                    break;
+                case "right_bumper":
+                    if (whiteCollectorToggle) {
+                        whiteCollectorToggle = false;
+                        robot.collectorWhite.setPower(0);
+                    } else {
+                        whiteCollectorToggle = true;
+                        robot.collectorWhite.setPower(1);
+                    }
+                    break;
+                case "guide":
+                    allianceRedDriver2 = !allianceRedDriver2;
+                    break;
+            }
+        }
+    }
+
+    private void turretOrbitControl (Gamepad gamepad, CRServo turretServo, boolean bias){
+
+        //  if either of these buttons... move the servo
+        //  turretServo1 = orange
+        if (gamepad.dpad_right || gamepad.dpad_left) {
+            if (bias) {
+                if (gamepad.dpad_right) {
+                    turretServo.setPower(-1);
+                }
+
+                if (gamepad.dpad_left) {
+                    turretServo.setPower(1);
+                }
+            }  else {
+                if (gamepad.dpad_right) {
+                    turretServo.setPower(1);
+                }
+
+                if (gamepad.dpad_left) {
+                    turretServo.setPower(-1);
+                }
+            }
+        } else {
+            turretServo.setPower(0);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/ServoTestEngine.java

@@ -0,0 +1,15 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.HardwareTesting.States.ISO8061State;
+import org.timecrafters.FreightFrenzy.HardwareTesting.States.ServoTestState;
+
+@TeleOp(name = "Servo Test ", group = "testing")
+public class ServoTestEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new ServoTestState());
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/TensorFlowTestEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.HardwareTesting.States.TensorFlowTestState;
+
+@TeleOp(name = "TensorFlow Test", group = "testing")
+public class TensorFlowTestEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        Robot robot = new Robot(this);
+
+        addState(new TensorFlowTestState(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/VuforiaTestEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.HardwareTesting.States.VuforiaTestState;
+
+@TeleOp(name = "Vuforia Test", group = "testing")
+public class VuforiaTestEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        Robot robot = new Robot(this);
+
+        addState(new VuforiaTestState(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/DriveTrainTestState.java

@@ -29,10 +29,10 @@ public class DriveTrainTestState extends CyberarmState {
     //This one is set up to repeat every few milliseconds
     @Override
     public void exec() {
-
-        robot.driveFrontRight.setPower(-engine.gamepad1.right_stick_y * maxSpeed);
-        robot.driveBackRight.setPower(-engine.gamepad1.right_stick_y * maxSpeed);
-        robot.driveFrontLeft.setPower(-engine.gamepad1.left_stick_y * maxSpeed);
-        robot.driveBackLeft.setPower(-engine.gamepad1.left_stick_y * maxSpeed);
+//
+//        robot.driveFrontRight.setPower(-engine.gamepad1.right_stick_y * maxSpeed);
+//        robot.driveBackRight.setPower(-engine.gamepad1.right_stick_y * maxSpeed);
+//        robot.driveFrontLeft.setPower(-engine.gamepad1.left_stick_y * maxSpeed);
+//        robot.driveBackLeft.setPower(-engine.gamepad1.left_stick_y * maxSpeed);
     }
 }

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/ServoTestState.java

@@ -0,0 +1,25 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.States;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class ServoTestState extends CyberarmState {
+
+    Servo transferServo;
+
+    public void init() {
+        transferServo = engine.hardwareMap.servo.get("Transfer_Servo");
+    }
+
+    @Override
+    public void exec() {
+        transferServo.setPosition(0.0);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("The ISO8061 Date Format is Best");
+        engine.telemetry.addLine("YYYY-MM-DD");
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/TensorFlowTestState.java

@@ -0,0 +1,38 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class TensorFlowTestState extends CyberarmState {
+    private Robot robot;
+
+    public TensorFlowTestState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        robot.activateTensorflow();
+    }
+
+    @Override
+    public void exec() {
+
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateTensorflow();
+    }
+
+    @Override
+    public void telemetry() {
+        for (Recognition recognition : robot.tensorflowDetections()) {
+            engine.telemetry.addData("Label", recognition.getLabel());
+            engine.telemetry.addData("Left", recognition.getLeft());
+            engine.telemetry.addData("Top", recognition.getTop());
+            engine.telemetry.addData("Confidence", recognition.getConfidence());
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/VuforiaTestState.java

@@ -0,0 +1,42 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.Competition.Common.RobotLocation;
+
+public class VuforiaTestState extends CyberarmState {
+    private Robot robot;
+
+    public VuforiaTestState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        robot.activateVuforia();
+    }
+
+    @Override
+    public void exec() {
+
+    }
+
+    @Override
+    public void telemetry() {
+        RobotLocation robotLocation = robot.vuforiaLocation();
+
+        engine.telemetry.addLine("Robot Vuforia Location");
+
+        if (robotLocation != null) {
+            engine.telemetry.addData("Orientation", "heading: %.2f, roll: %.2f, pitch: %.2f", robotLocation.heading, robotLocation.roll, robotLocation.pitch);
+            engine.telemetry.addData("Location", "X: %.2f, Y: %.2f, Z: %.2f", robotLocation.x, robotLocation.y, robotLocation.z);
+        } else {
+            engine.telemetry.addLine("No Data");
+        }
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java

@@ -85,8 +85,8 @@ public class TensorFlowCheck extends CyberarmState {
 
              setHasFinished(true);
          }
-    }
 
+    }
     @Override
     public void telemetry() {
         engine.telemetry.addData("Chosen Path", path);

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java

@@ -8,7 +8,7 @@ import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
 import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 @Disabled
-@TeleOp (name = "TeleOp",group = "comp")
+//@TeleOp (name = "TeleOp",group = "comp")
 public class TeleOpEngine extends CyberarmEngine {
 
     private Robot robot;