Fixed up tele op and robot object

(:

.idea/.gitignore

@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml

.idea/compiler.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CompilerConfiguration">
+    <bytecodeTargetLevel target="11" />
+  </component>
+</project>

.idea/jarRepositories.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="RemoteRepositoriesConfiguration">
+    <remote-repository>
+      <option name="id" value="central" />
+      <option name="name" value="Maven Central repository" />
+      <option name="url" value="https://repo1.maven.org/maven2" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="jboss.community" />
+      <option name="name" value="JBoss Community repository" />
+      <option name="url" value="https://repository.jboss.org/nexus/content/repositories/public/" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="MavenRepo" />
+      <option name="name" value="MavenRepo" />
+      <option name="url" value="https://repo.maven.apache.org/maven2/" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="BintrayJCenter" />
+      <option name="name" value="BintrayJCenter" />
+      <option name="url" value="https://jcenter.bintray.com/" />
+    </remote-repository>
+    <remote-repository>
+      <option name="id" value="Google" />
+      <option name="name" value="Google" />
+      <option name="url" value="https://dl.google.com/dl/android/maven2/" />
+    </remote-repository>
+  </component>
+</project>

.idea/misc.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ExternalStorageConfigurationManager" enabled="true" />
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="11" project-jdk-type="JavaSDK">
+    <output url="file://$PROJECT_DIR$/build/classes" />
+  </component>
+  <component name="ProjectType">
+    <option name="id" value="Android" />
+  </component>
+</project>

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/engine/V2/GamepadChecker.java

@@ -41,7 +41,10 @@ public class GamepadChecker {
         Field field = gamepad.getClass().getDeclaredField(btn);
 
         if (field.getBoolean(gamepad)) {
-          engine.buttonDown(gamepad, btn);
+          if (!buttons.get(btn)) {
+            engine.buttonDown(gamepad, btn);
+          }
+
           buttons.put(btn, true);
         } else {
           if (buttons.get(btn)) {

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueDuckEngine.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.DriveState;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TensorFlowState;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+@Autonomous (name = "Blue Duck Autonomous", group = "blue")
+public class BlueDuckEngine extends CyberarmEngine {
+    Robot robot;
+
+    @Override
+    public void setup() {
+        this.robot = new Robot(this);
+        robot.resetEncoders();
+
+        addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "BlueDuckAutonomous", "01_0"));
+        addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "BlueDuckAutonomous", "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, "BlueDuckAutonomous", "05_0"));
+        addState(new CollectorToggle(robot, robot.collectorWhite, "BlueDuckAutonomous", "06_0"));
+        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueDuckAutonomous", "07_0"));
+        addState(new DriveState(robot,"BlueDuckAutonomous", "08_0"));
+        addState(new DriveState(robot, "BlueDuckAutonomous", "09_0"));
+        addState(new TurretOrbit(robot, robot.turretServoWhite, null, "BlueDuckAutonomous", "10_0"));
+        addState(new DriveState(robot, "BlueDuckAutonomous", "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/Engines/BlueWarehouseEngine.java

@@ -0,0 +1,60 @@
+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);
+        robot.resetEncoders();
+
+        addState(new TurretOrbit(robot, robot.turretServoOrange, robot.orangeMag, "BlueWarehouseAutonomous", "01_0"));
+        addState(new TensorFlowState(robot, robot.orangeArmRiser, robot.orangeArmBobbin, "BlueWarehouseAutonomous", "01_1"));
+//        addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "02_0"));
+//        addState(new TurretArmRiser(robot, robot.orangeArmRiser, "RedWarehouseAutonomous", "03_0_middle"));
+//        addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "04_0"));
+        addState(new CollectorToggle(robot, robot.collectorOrange, "BlueWarehouseAutonomous", "05_0"));
+        addState(new CollectorToggle(robot, robot.collectorOrange, "BlueWarehouseAutonomous", "06_0"));
+        addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "BlueWarehouseAutonomous", "07_0"));
+        addState(new DriveState(robot,"BlueWarehouseAutonomous", "08_0"));
+        addState(new DriveState(robot, "BlueWarehouseAutonomous", "09_0"));
+        addState(new TurretOrbit(robot, robot.turretServoOrange, null, "BlueWarehouseAutonomous", "10_0"));
+        addState(new DriveState(robot, "BlueWarehouseAutonomous", "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/Engines/RedDuckEngine.java

@@ -0,0 +1,36 @@
+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.TurretOrbit;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+@Autonomous(name = "RedDuckAutonomous", group = "red")
+
+public class RedDuckEngine extends CyberarmEngine {
+
+    @Override
+    public void setup() {
+        Robot robot = new Robot(this);
+        robot.resetEncoders();
+
+        addState(new TurretOrbit(robot, robot.turretServoOrange, robot.orangeMag, "RedDuckAutonomous", "01_0"));
+        addState(new TensorFlowState(robot, robot.orangeArmRiser, robot.orangeArmBobbin, "RedDuckAutonomous", "01_1"));
+//        addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "02_0"));
+//        addState(new TurretArmRiser(robot, robot.orangeArmRiser, "RedWarehouseAutonomous", "03_0_middle"));
+//        addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "04_0"));
+        addState(new CollectorToggle(robot, robot.collectorOrange, "RedDuckAutonomous", "05_0"));
+        addState(new CollectorToggle(robot, robot.collectorOrange, "RedDuckAutonomous", "06_0"));
+        addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedDuckAutonomous", "07_0"));
+        addState(new DriveState(robot,"RedDuckAutonomous", "08_0"));
+        addState(new DriveState(robot, "RedDuckAutonomous", "09_0"));
+        addState(new TurretOrbit(robot, robot.turretServoOrange, null, "RedDuckAutonomous", "10_0"));
+        addState(new DriveState(robot, "RedDuckAutonomous", "10_1"));
+
+    }
+}

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

@@ -1,14 +1,60 @@
 package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines;
 
-import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState;
-import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+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() {
-        Robot robot = new Robot(this);
+        this.robot = new Robot(this);
+        robot.resetEncoders();
 
-        addState(new DriveState(robot, 2000, 2000,.25,.25));
+        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;
+    double 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

@@ -7,14 +7,15 @@ import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
 
 public class DriveState extends CyberarmState {
     Robot robot;
-    double distanceLeft, distanceRight, powerLeft, powerRight;
+    int distanceLeft, distanceRight;
+    double powerLeft, powerRight;
 
-    public DriveState(Robot robot, double distanceLeft, double distanceRight, double powerLeft, double powerRight) {
+    public DriveState(Robot robot, String groupName, String actionName) {
         this.robot = robot;
-        this.distanceLeft = distanceLeft;
-        this.distanceRight = distanceRight;
-        this.powerLeft = powerLeft;
-        this.powerRight = powerRight;
+        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
@@ -32,7 +33,8 @@ public class DriveState extends CyberarmState {
 
     @Override
     public void exec() {
-        if (Math.abs(robot.driveGoalLeft.getCurrentPosition()) >= distanceLeft && Math.abs(robot.driveGoalRight.getCurrentPosition()) >= distanceRight) {
+        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);
@@ -45,6 +47,15 @@ public class DriveState extends CyberarmState {
             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,100 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import android.util.Log;
+
+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 = 0;
+    private int path = 3;
+    private double leftDuck;
+    private double middleDuck;
+
+    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;
+        this.leftDuck = robot.configuration.variable(groupName, actionName, "leftDuck").value();
+        this.middleDuck = robot.configuration.variable(groupName, actionName, "middleDuck").value();
+        this.checkTime = robot.configuration.variable(groupName, actionName, "time").value();
+    }
+
+    @Override
+    public void start() {
+        robot.activateVuforia();
+        robot.activateTensorflow();
+    }
+
+    @Override
+    public void exec() {
+        recognitions = robot.tensorflowDetections();
+
+        if (runTime() < checkTime) {
+            if (recognitions != null && recognitions.size() != 0) {
+                if (recognitions.size() == 1) {
+                    Recognition recognition = recognitions.get(0);
+
+                    if (recognition.getLeft() < leftDuck) {
+                        path = 0;
+                    } else {
+                        path = 1;
+                    }
+                }
+            } else {
+                path = 2;
+            }
+        } else {
+            Log.i(TAG, "Choosen path: " + path);
+
+            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() {
+        engine.telemetry.addData("Runtime", runTime());
+        engine.telemetry.addData("Check Time", checkTime);
+        engine.telemetry.addData("Path", path);
+        engine.telemetry.addLine();
+
+        if (recognitions == null) {
+            return;
+        }
+
+        for (Recognition recognition : recognitions) {
+            engine.telemetry.addData("Label", recognition.getLabel());
+            engine.telemetry.addData("Left", recognition.getLeft());
+            engine.telemetry.addData("Top", recognition.getTop());
+            engine.telemetry.addData("Confidence", recognition.getConfidence());
+            engine.telemetry.addLine();
+        }
+    }
+}

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

@@ -0,0 +1,42 @@
+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 exec() {
+        if (motor.getCurrentPosition() < targetPosition - tolerance){
+            motor.setPower(power);
+        }
+
+        else if (motor.getCurrentPosition() > targetPosition + tolerance){
+            motor.setPower(-power);
+        }
+        else {
+            setHasFinished(true);
+            motor.setPower(0);
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("targetPosition", targetPosition);
+    }
+}

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

@@ -0,0 +1,37 @@
+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 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 != null && magnetSwitch.isPressed()) || runTime() > time ){
+      servo.setPower(0);
+      setHasFinished(true);
+  }
+  else {
+      servo.setPower(power);
+  }
+    }
+}

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

@@ -6,6 +6,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.
 
 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;
@@ -65,31 +66,34 @@ public class Robot {
             .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 driveWarehouseLeft, driveWarehouseRight, driveGoalLeft, driveGoalRight,
-            collectorArmRiser, collectorArmBobbin, depositorArmRiser, depositorArmBobbin;
+            orangeArmRiser, orangeArmBobbin, whiteArmRiser, whiteArmBobbin;
 
     // Collector
-    public Servo collectorDispenser;
-    public CRServo turretServo1;
+    public Servo orangeDispenser;
+    public CRServo turretServoOrange;
 
     // Depositor
-    public Servo depositorDispenser;
+    public Servo whiteDispenser;
     // Depositor, and carousel
-    public CRServo turretServo2, carouselWheel, collectorOrange, collectorWhite;
+    public CRServo turretServoWhite, carouselWheel, collectorOrange, collectorWhite;
 
 
 
     public Robot(CyberarmEngine engine) {
         this.engine = engine;
-
+        initMagnetSwitches();
         initConfiguration();
         initBlinkin();
         initIMU();
@@ -97,20 +101,29 @@ public class Robot {
         initCollector();
         initDepositor();
         initCarousel();
-//        initVuforia();
-//        initTensorflow();
+        initVuforia();
+        initTensorflow();
+    }
+
+    private void initMagnetSwitches() {
+        whiteMag = engine.hardwareMap.get(RevTouchSensor.class, "whiteMag");
+        orangeMag = engine.hardwareMap.get(RevTouchSensor.class, "orangeMag");
     }
 
     public double heading() {
-        return imu.getAngularOrientation().firstAngle;
+        return orientation.firstAngle;
     }
 
     public double roll() {
-        return imu.getAngularOrientation().secondAngle;
+        return orientation.secondAngle;
     }
 
     public double pitch() {
-        return imu.getAngularOrientation().thirdAngle;
+        return orientation.thirdAngle;
+    }
+
+    public void updateRobotOrientation(){
+        orientation = imu.getAngularOrientation();
     }
 
     public void activateTensorflow() {
@@ -207,24 +220,26 @@ public class Robot {
     }
 
     private void initCollector() {
-        collectorArmBobbin = engine.hardwareMap.dcMotor.get("collectorArmBobbin");
-        collectorDispenser = engine.hardwareMap.servo.get("collectorDispenser");
-        collectorArmBobbin.setDirection(DcMotorSimple.Direction.FORWARD);
-        turretServo1 = engine.hardwareMap.crservo.get("turretServo1");
-        collectorArmRiser = engine.hardwareMap.dcMotor.get("collectorArmRiser");
-        collectorArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
-        collectorArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        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(){
-        depositorDispenser = engine.hardwareMap.servo.get("depositorDispenser");
-        depositorArmBobbin = engine.hardwareMap.dcMotor.get("depositorArmBobbin");
-        depositorArmBobbin.setDirection(DcMotorSimple.Direction.REVERSE);
-        turretServo2 = engine.hardwareMap.crservo.get("turretServo2");
-        depositorArmRiser = engine.hardwareMap.dcMotor.get("depositorArmRiser");
-        depositorArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
-        depositorArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        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");
     }
 
@@ -259,6 +274,18 @@ public class Robot {
         }
     }
 
+    public void  resetEncoders(){
+        orangeArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        orangeArmRiser.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        whiteArmRiser.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        orangeArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        orangeArmRiser.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        whiteArmRiser.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
     private void initTensorflow() {
         int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
                 "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());

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

@@ -1,5 +1,5 @@
 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;

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

@@ -1,6 +1,8 @@
 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;
@@ -8,18 +10,28 @@ 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 = 0.7;
-     maxDepositorArmSpeed = 0.7;
+     maxCollectorArmSpeed = 1;
+     maxDepositorArmSpeed = 1;
+     minArmTravelDistance = 0;
+     maxArmTravelDistance = 4000;
+
     }
 
+
     @Override
     public void start() {
         super.start();
+        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
@@ -34,152 +46,237 @@ public class TeleOpState extends CyberarmState {
         robot.driveGoalRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed);
 
         // dispenser powered
-        if (engine.gamepad1.b){
-            robot.collectorDispenser.setPosition(.5);
+        if (engine.gamepad1.a){
+            robot.orangeDispenser.setPosition(0);
         }
-
         // if not pressed dispenser off
         else {
-            robot.collectorDispenser.setPosition(0);
-        }
-        //  if one of triggers pressed arm extends or unextends
-        robot.collectorArmBobbin.setPower(engine.gamepad1.left_trigger * maxCollectorArmSpeed);
-
-        if (engine.gamepad1.left_trigger <= 0){
-            robot.collectorArmBobbin.setPower(-engine.gamepad1.right_trigger * maxCollectorArmSpeed);
+            robot.orangeDispenser.setPosition(0.5);
         }
 
-        //  if either of these buttons... move the servo
-        //  turretServo1 = orange
-        if (engine.gamepad1.dpad_right || engine.gamepad1.dpad_left) {
+        //  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) {
 
-            if (engine.gamepad1.dpad_right) {
-                robot.turretServo1.setPower(-1);
+            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);
             }
 
-            if (engine.gamepad1.dpad_left) {
-                robot.turretServo1.setPower(1);
-            }
         }
 
-        //  if neither of these buttons... power off
-        else {
-            robot.turretServo1.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.collectorArmRiser.setPower(1);
+                robot.orangeArmRiser.setPower(1);
             }
 
             if (engine.gamepad1.dpad_down) {
-                robot.collectorArmRiser.setPower(-1);
+                robot.orangeArmRiser.setPower(-1);
             }
         }
         // nothing pressed nothing move...
         else {
-            robot.collectorArmRiser.setPower(0);
+            robot.orangeArmRiser.setPower(0);
         }
 
-        // if bumpers pressed intake wheel move...
+        // button x pressed carousel wheel move, for blue side, if b pressed wheel spins for red side;
 
-        if (engine.gamepad1.right_bumper || engine.gamepad1.left_bumper) {
-            if (engine.gamepad1.right_bumper) {
-                robot.collectorOrange.setPower(1);
-            }
-
-            if (engine.gamepad1.left_bumper) {
-                robot.collectorOrange.setPower(-1);
-            }
-        }
-        // no bumpers pressed no wheel move...
-        else {
-            robot.collectorOrange.setPower(0);
-        }
-
-        // button x pressed carousel wheel move.
-
-        if (engine.gamepad1.x){
+        if (engine.gamepad2.b){
             robot.carouselWheel.setPower(1);
         }
-
+        else if (engine.gamepad2.x){
+            robot.carouselWheel.setPower(-1);
+        }
         else {
             robot.carouselWheel.setPower(0);
         }
 
         // GamePad 2
 
-        // if triggers are pressed then arm extends or unextends
-        robot.depositorArmBobbin.setPower(engine.gamepad2.left_trigger * maxDepositorArmSpeed);
+        // 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 (engine.gamepad2.left_trigger <= 0) {
-            robot.depositorArmBobbin.setPower(-engine.gamepad2.right_trigger * maxDepositorArmSpeed);
         }
 
+
+
         // if b is pressed then depositor door opens, if not pressed not opening.
-        if (engine.gamepad2.b){
-            robot.depositorDispenser.setPosition(.5);
+        if (engine.gamepad2.a){
+            robot.whiteDispenser.setPosition(.5);
         }
 
         else {
-            robot.depositorDispenser.setPosition(0);
+            robot.whiteDispenser.setPosition(0);
         }
 
-        //  if either of these buttons move the servo
-        //  turretServo2 = white
-        if (engine.gamepad2.dpad_right || engine.gamepad2.dpad_left) {
-
-            if (engine.gamepad2.dpad_right) {
-                robot.turretServo2.setPower(-1);
-            }
-
-            if (engine.gamepad2.dpad_left) {
-                robot.turretServo2.setPower(1);
-            }
-        }
-        //  if neither of these buttons power off
-        else {
-            robot.turretServo2.setPower(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.depositorArmRiser.setPower(1);
+                robot.whiteArmRiser.setPower(1);
             }
 
             if (engine.gamepad2.dpad_down) {
-                robot.depositorArmRiser.setPower(-1);
+                robot.whiteArmRiser.setPower(-1);
             }
         }
         // nothing pressed nothing move...
         else {
-            robot.depositorArmRiser.setPower(0);
+            robot.whiteArmRiser.setPower(0);
         }
-
-        if (engine.gamepad2.left_bumper || engine.gamepad2.right_bumper) {
-
-            if (engine.gamepad2.left_bumper) {
-                robot.collectorWhite.setPower(-1);
-            }
-
-            if (engine.gamepad2.right_bumper) {
-                robot.collectorWhite.setPower(1);
-            }
-        }
-
-        else {
-            robot.collectorWhite.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/MecanumRobot_Spencer/MecanumRobot.java

@@ -0,0 +1,37 @@
+package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+public class MecanumRobot {
+
+    private final CyberarmEngine engine;
+
+    // Drivetrain
+
+    public DcMotor driveFrontLeft, driveFrontRight, driveBackLeft, driveBackRight;
+
+
+
+    public MecanumRobot(CyberarmEngine engine) {
+        this.engine = engine;
+        initDrivetrain();
+    }
+    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");
+
+        driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+    }
+}
+
+

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/Mecanum_Drive_Engine.java

@@ -0,0 +1,13 @@
+package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+
+@TeleOp(name = "TeleOp Mecanum Robot")
+
+public class Mecanum_Drive_Engine extends CyberarmEngine {
+    @Override
+    public void setup() { addState(new TeleOpStateMecanumRobot(new MecanumRobot(this)));}
+
+    }

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java

@@ -0,0 +1,117 @@
+package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.MecanumRobot_Spencer.MecanumRobot;
+
+public class TeleOpStateMecanumRobot extends CyberarmState {
+
+    MecanumRobot robot;
+
+    public TeleOpStateMecanumRobot(MecanumRobot mecanumRobot) {
+
+    }
+
+    @Override
+    public void exec() {
+
+        // Slow Mode
+
+        if (engine.gamepad1.left_trigger > 0.5){
+        // TankDrive
+        // Left joystick forward; Left side forward;
+
+        robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+        robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+
+        // Right joystick forward; Right side forward;
+        robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+        robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+
+
+        // Strafing left and right
+
+        if (engine.gamepad1.left_bumper) {
+            robot.driveFrontLeft.setPower(0.5);
+            robot.driveBackLeft.setPower(-0.5);
+            robot.driveFrontRight.setPower(-0.5);
+            robot.driveBackRight.setPower(0.5);
+        } else if (engine.gamepad1.right_bumper) {
+            robot.driveFrontLeft.setPower(-0.5);
+            robot.driveBackLeft.setPower(0.5);
+            robot.driveFrontRight.setPower(0.5);
+            robot.driveBackRight.setPower(-0.5);
+    }
+//        else {
+//            robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+//            robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+//            robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+//            robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+
+
+       // }
+    // Normal Speed
+
+        if (engine.gamepad1.right_stick_button) {
+
+            // left stick forward; right side foward
+            robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+            robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+
+            // right stick foward; right side foward;
+            robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+            robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+
+        }
+            // Strafing left and right
+
+            if (engine.gamepad1.left_bumper) {
+                robot.driveFrontLeft.setPower(1);
+                robot.driveBackLeft.setPower(-1);
+                robot.driveFrontRight.setPower(-1);
+                robot.driveBackRight.setPower(1);
+            } else if (engine.gamepad1.right_bumper) {
+                robot.driveFrontLeft.setPower(-1);
+                robot.driveBackLeft.setPower(1);
+                robot.driveFrontRight.setPower(1);
+                robot.driveBackRight.setPower(-1);
+            } else {
+                robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+                robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+                robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+                robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+
+            }
+        }
+
+        else {
+            // left stick foward; right side foward
+            robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+            robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+
+            // right stick foward; right side foward;
+            robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+            robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+
+
+            // Strafing left and right
+
+//            if (engine.gamepad1.left_bumper) {
+//                robot.driveFrontLeft.setPower(1);
+//                robot.driveBackLeft.setPower(-1);
+//                robot.driveFrontRight.setPower(-1);
+//                robot.driveBackRight.setPower(1);
+//            } else if (engine.gamepad1.right_bumper) {
+//                robot.driveFrontLeft.setPower(-1);
+//                robot.driveBackLeft.setPower(1);
+//                robot.driveFrontRight.setPower(1);
+//                robot.driveBackRight.setPower(-1);
+//            } else {
+//                robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+//                robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+//                robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+//                robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+//
+//            }
+        }
+    }
+}

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