Merge remote-tracking branch 'origin/master'

# Conflicts:
#	TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java

FtcRobotController/build.gradle

@@ -8,7 +8,7 @@ apply plugin: 'com.android.library'
 android {
 
     defaultConfig {
-        minSdkVersion 23
+        minSdkVersion 24
         //noinspection ExpiredTargetSdkVersion
         targetSdkVersion 28
         buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"'

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

@@ -411,7 +411,7 @@ public abstract class CyberarmEngine extends OpMode {
    * state must have a construction that takes 3 arguments: object, groupName, and actionName
    * @param configuration TimeCraftersConfiguration
    * @param packageName Package name where states are defined
-   * @param object Object to pass to as first argument to states constructor
+   * @param object Object to pass as first argument to states constructor
    * @param objectClass Class to cast object to
    * @param groupName Group name
    */
@@ -456,7 +456,10 @@ public abstract class CyberarmEngine extends OpMode {
       } catch (ClassNotFoundException | NoSuchMethodException | IllegalAccessException | InstantiationException | InvocationTargetException e) {
         e.printStackTrace();
 
-        throw(new RuntimeException(e));
+        RuntimeException exception = new RuntimeException(e.getMessage(), e.getCause());
+        exception.setStackTrace(e.getStackTrace());
+
+        throw(exception);
       }
     }
   }

TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java

@@ -1,51 +0,0 @@
-package org.timecrafters.Autonomous.Engines;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.cyberarm.engine.V2.CyberarmEngine;
-import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-@Autonomous(name = "Right Side")
-public class RightStateAutoEngine extends CyberarmEngine {
-    PhoenixBot1 robot;
-
-    @Override
-    public void setup() {
-        robot = new PhoenixBot1(this);
-
-        // driving towards Low
-        addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
-        // rotate towards low
-        addState(new RotationState(robot, "Right State Auto", "02-1"));
-        // drive forwards or backwards to adjust to pole
-        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
-        // counteract distance driven
-        addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
-        // rotate towards opposing alliance
-        addState(new RotationState(robot, "Right State Auto", "04-1"));
-        // drive to stack column
-        addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0"));
-        // rotate at stack
-        addState(new RotationState(robot, "Right State Auto", "05-1"));
-        // drive at stack
-//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
-
-
-
-
-
-
-
-    }
-}

TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java

@@ -1,109 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class DriverParkPlaceState extends CyberarmState {
-    private final boolean stateDisabled;
-    PhoenixBot1 robot;
-    private int RampUpDistance;
-    private int RampDownDistance;
-    private String intendedPlacement;
-
-    public DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
-        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
-        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
-        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
-        this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
-        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
-    }
-    private double drivePower, targetDrivePower;
-    private int traveledDistance;
-
-    @Override
-    public void start() {
-        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-    }
-
-    @Override
-    public void exec() {
-        if (stateDisabled) {
-            setHasFinished(true);
-            return;
-        }
-        String placement = engine.blackboardGetString("parkPlace");
-        if (placement != null) {
-            if (!placement.equals(intendedPlacement)){
-                    setHasFinished(true);
-            }
-            if (placement.equals(intendedPlacement)) {
-                double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
-
-                if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
-                    // ramping up
-                    drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.25;
-                } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
-                    // ramping down
-                    drivePower = ((delta / RampDownDistance) + 0.25);
-                } else {
-                    // middle ground
-                    drivePower = targetDrivePower;
-                }
-
-                if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
-                    // This is limiting drive power to the targeted drive power
-                    drivePower = targetDrivePower;
-                }
-
-                if (targetDrivePower < 0 && drivePower != targetDrivePower) {
-                    drivePower = drivePower * -1;
-                }
-
-                if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
-                    robot.backLeftDrive.setPower(drivePower);
-                    robot.backRightDrive.setPower(drivePower);
-                    robot.frontLeftDrive.setPower(drivePower);
-                    robot.frontRightDrive.setPower(drivePower);
-                } else {
-                    robot.backLeftDrive.setPower(0);
-                    robot.backRightDrive.setPower(0);
-                    robot.frontLeftDrive.setPower(0);
-                    robot.frontRightDrive.setPower(0);
-                    setHasFinished(true);
-                } // else ending
-            } // placement equals if statement
-//            setHasFinished(true);
-        } // end of placement doesn't equal null
-
-    } // end of exec
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("Position", intendedPlacement);
-        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
-        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
-
-        engine.telemetry.addData("drivePower", drivePower);
-        engine.telemetry.addData("targetDrivePower", targetDrivePower);
-
-        engine.telemetry.addData("traveledDistance", traveledDistance);
-        engine.telemetry.addData("RampUpDistance", RampUpDistance);
-        engine.telemetry.addData("RampDownDistance", RampDownDistance);
-
-    } // end of telemetry
-} // end of class

TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java

@@ -1,244 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import android.util.Log;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class DriverStateWithOdometer extends CyberarmState {
-    private final boolean stateDisabled;
-    PhoenixBot1 robot;
-    private double RampUpDistance;
-    private double RampDownDistance;
-    private double maximumTolerance;
-    private float direction;
-    private boolean targetAchieved = false;
-    private double CurrentPosition;
-    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
-    public final int COUNTS_PER_REVOLUTION = 8192;
-    public double MINIMUM_POWER = 0.25;
-    public final double distanceMultiplier;
-    public double startOfRampUpRight;
-    public double startOfRampDownRight;
-    public double startOfRampUpLeft;
-    public double startOfRampDownLeft;
-    public double endOfRampUpRight;
-    public double endOfRampDownRight;
-    public double endOfRampUpLeft;
-    public double endOfRampDownLeft;
-
-    public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
-        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
-        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
-        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
-        this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
-        this.direction = robot.configuration.variable(groupName, actionName, "direction").value();
-        this.distanceMultiplier = robot.configuration.variable(groupName, actionName, "distanceMultiplier").value();
-
-        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
-    }
-
-    private double drivePower, targetDrivePower;
-    private int traveledDistance;
-
-    @Override
-    public void start() {
-
-        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
-        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
-        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
-        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
-
-
-        if (drivePower > 0) {
-            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
-            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
-            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
-            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
-
-            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
-            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
-            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
-            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
-        } else {
-
-            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
-            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
-            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
-            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
-
-            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
-            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
-            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
-            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
-
-        }
-
-    }
-
-    @Override
-    public void exec() {
-
-
-        if (stateDisabled) {
-            setHasFinished(true);
-            return;
-        }
-
-        double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
-        double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
-
-        // ramping up
-        if (RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight &&
-                LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft) {
-
-            if (targetDrivePower > 0) {
-                drivePower = (targetDrivePower - MINIMUM_POWER) * ((RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + MINIMUM_POWER;
-            } else {
-                drivePower = (targetDrivePower + MINIMUM_POWER) * ((startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - MINIMUM_POWER;
-            }
-        }
-
-        // Driving Normal
-        else if (RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight &&
-                LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft) {
-
-            drivePower = targetDrivePower;
-
-        }
-
-        // Ramping down
-        else if (RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight &&
-                LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft) {
-            if (targetDrivePower > 0) {
-                drivePower = (targetDrivePower + MINIMUM_POWER) * ((startOfRampDownRight - RightCurrentPosition) / RampDownDistance) - MINIMUM_POWER;
-            } else {
-                drivePower = (targetDrivePower - MINIMUM_POWER) * ((RightCurrentPosition - startOfRampDownRight) / RampDownDistance) + MINIMUM_POWER;
-            }
-        }
-            if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
-                // This is limiting drive power to the targeted drive power
-                drivePower = targetDrivePower;
-            }
-
-            if (targetDrivePower < 0 && drivePower > 0) {
-                drivePower = drivePower * -1;
-            }
-
-            if (LeftCurrentPosition - traveledDistance < maximumTolerance || Math.abs(RightCurrentPosition - traveledDistance) < maximumTolerance) {
-                if (targetAchieved) {
-                    drivePower = drivePower * 0.15;
-
-                    if (Math.abs(drivePower) < 0.15) {
-                        if (drivePower < 0) {
-                            drivePower = -0.15;
-                        } else {
-                            drivePower = 0.15;
-                        }
-                    }
-                }
-                robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
-                robot.backRightDrive.setPower(drivePower);
-                robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
-                robot.frontRightDrive.setPower(drivePower);
-
-            } else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
-                targetAchieved = true;
-
-                drivePower = targetDrivePower * -0.15;
-
-                if (Math.abs(drivePower) < 0.15) {
-                    if (drivePower < 0) {
-                        drivePower = -0.15;
-                    } else {
-                        drivePower = 0.15;
-                    }
-                }
-
-                robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(drivePower);
-                robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(drivePower);
-
-
-            } else {
-
-                if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)) {
-
-                    if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
-                        drivePower = 0;
-                    } else {
-                        drivePower = 0.15;
-                        robot.backLeftDrive.setPower(-drivePower);
-                        robot.backRightDrive.setPower(drivePower);
-                        robot.frontLeftDrive.setPower(-drivePower);
-                        robot.frontRightDrive.setPower(drivePower);
-                    }
-                }
-
-                if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)) {
-
-                    if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)) {
-                        drivePower = 0;
-                    } else {
-                        drivePower = 0.15;
-                        robot.backLeftDrive.setPower(drivePower);
-                        robot.backRightDrive.setPower(-drivePower);
-                        robot.frontLeftDrive.setPower(drivePower);
-                        robot.frontRightDrive.setPower(-drivePower);
-                    }
-                } else {
-                    robot.backLeftDrive.setPower(0);
-                    robot.backRightDrive.setPower(0);
-                    robot.frontLeftDrive.setPower(0);
-                    robot.frontRightDrive.setPower(0);
-                    setHasFinished(true);
-                }
-            }
-
-//
-        }
-
-        @Override
-        public void telemetry () {
-            engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
-            engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
-            engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
-            engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-            engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
-            engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
-            engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
-            engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
-            engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
-            engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
-            engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
-            engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
-
-            engine.telemetry.addData("Target Achieved", targetAchieved);
-
-
-            engine.telemetry.addData("drivePower", drivePower);
-            engine.telemetry.addData("targetDrivePower", targetDrivePower);
-
-            engine.telemetry.addData("traveledDistance", traveledDistance);
-            engine.telemetry.addData("RampUpDistance", RampUpDistance);
-            engine.telemetry.addData("RampDownDistance", RampDownDistance);
-
-            Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
-            Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
-            Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
-
-        }
-    }
-

TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java

@@ -1,100 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class RotationState extends CyberarmState {
-    private final boolean stateDisabled;
-    PhoenixBot1 robot;
-    public RotationState(PhoenixBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
-        this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
-        drivePowerVariable = drivePower;
-        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
-
-
-    }
-
-    private double drivePower;
-    private float targetRotation;
-    float RobotRotation;
-    private double RotationTarget;
-    private double RotationDirectionMinimum;
-    private String debugStatus = "?";
-    private double drivePowerVariable;
-    private double leftCompensator;
-    private double RightCompensator;
-
-    @Override
-    public void start() {
-
-        leftCompensator = robot.OdometerEncoderLeft.getCurrentPosition();
-        RightCompensator = robot.OdometerEncoderRight.getCurrentPosition();
-    }
-
-    @Override
-    public void exec() {
-        if (stateDisabled){
-            setHasFinished(true);
-            return;
-            } // end of if
-
-        RobotRotation = robot.imu.getAngularOrientation().firstAngle;
-
-        if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
-            drivePowerVariable = 0.4 * drivePower;
-            if (Math.abs(drivePowerVariable) < 0.4) {
-                if (drivePowerVariable < 0){
-                    drivePowerVariable = -0.4;
-                } else {
-                    drivePowerVariable = 0.4;
-                }
-            }
-            debugStatus = "Rotate Slow";
-        } // end of if
-        else {
-            drivePowerVariable = drivePower * 0.75;
-            debugStatus = "Rotate Fast";
-        }
-
-        if (RobotRotation >= targetRotation + 1){
-            drivePowerVariable = Math.abs(drivePowerVariable);
-        } else {
-            drivePowerVariable = -1 * Math.abs(drivePowerVariable);
-        }
-
-        if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) {
-            robot.backLeftDrive.setPower(drivePowerVariable);
-            robot.backRightDrive.setPower(-drivePowerVariable);
-            robot.frontLeftDrive.setPower(drivePowerVariable);
-            robot.frontRightDrive.setPower(-drivePowerVariable);
-        } else
-        {
-            robot.backLeftDrive.setPower(0);
-            robot.backRightDrive.setPower(0);
-            robot.frontLeftDrive.setPower(0);
-            robot.frontRightDrive.setPower(0);
-            PhoenixBot1.leftCompensatorGlobal = (leftCompensator + robot.OdometerEncoderLeft.getCurrentPosition()) - leftCompensator;
-            PhoenixBot1.RightCompensatorGlobal = (RightCompensator + robot.OdometerEncoderRight.getCurrentPosition()) - RightCompensator;
-            setHasFinished(true);
-        }
-
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("DEBUG Status", debugStatus);
-
-        engine.telemetry.addLine();
-
-        engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
-        engine.telemetry.addData("Robot Target Rotation", targetRotation);
-        engine.telemetry.addData("Drive Power", drivePowerVariable);
-        engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
-        engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
-        engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
-        engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
-
-    }
-}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftSideAutonomousEngine.java

@@ -1,22 +1,21 @@
-package org.timecrafters.Autonomous.Engines;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverState;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverState;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
-@Autonomous (name = "Left Side")
+//@Autonomous (name = "Left Side")
 @Disabled
 
 public class LeftSideAutonomousEngine extends CyberarmEngine {

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftStateAutoEngine.java

@@ -0,0 +1,32 @@
+package org.timecrafters.Phoenix.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+@Autonomous(name = "Left Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
+public class LeftStateAutoEngine extends CyberarmEngine {
+    PhoenixBot1 robot;
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
+    }
+
+    @Override
+    public void setup() {
+        robot = new PhoenixBot1(this);
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.configuration,
+                "org.timecrafters.Autonomous.States",
+                robot,
+                PhoenixBot1.class,
+                "Left State Auto");
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftTwoConeAutonomousEngine.java

@@ -1,23 +1,21 @@
-package org.timecrafters.Autonomous.Engines;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
-import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
-import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverStateWithOdometer;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentAngleState;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
-@Autonomous (name = "left 2 cone auto")
+//@Autonomous (name = "left 2 cone auto")
 
 public class LeftTwoConeAutonomousEngine extends CyberarmEngine {
     PhoenixBot1 robot;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightSideAutonomousEngine.java

@@ -1,19 +1,19 @@
-package org.timecrafters.Autonomous.Engines;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverState;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverState;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 @Autonomous (name = "Right ")
 @Disabled

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightStateAutoEngine.java

@@ -0,0 +1,128 @@
+package org.timecrafters.Phoenix.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+@Autonomous(name = "Right Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
+public class RightStateAutoEngine extends CyberarmEngine {
+    PhoenixBot1 robot;
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
+    }
+
+    @Override
+    public void setup() {
+        robot = new PhoenixBot1(this);
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.configuration,
+                "org.timecrafters.Autonomous.States",
+                robot,
+                PhoenixBot1.class,
+                "Right State Auto");
+
+////        addState(new TopArmv2(robot, "Right State Auto", "06-0"));
+//
+////        // forward to low junction ..........................................................................(I have PreLoaded Cone)
+////        addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
+//
+//        // rotate left towards low junction (angle = 45, direction = CCW) (I have PreLoaded Cone)
+//        addState(new RotationState(robot, "Right State Auto", "02-1"));
+//
+//        // driving forward / backwards to adjust (I have PreLoaded Cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
+//
+//        // counteract distance driven .........................................................................(I'm going for 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
+//
+//        // rotate towards opposing alliance (angle = 0, direction = CW) (I'm going for 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "05-0"));
+//
+//        // drive to tall junction (to adjust to cone stack)  (I'm going for 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
+//
+//        // rotate at stack (angle = -90, direction = CW) (I'm going for 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "07-0"));
+//
+//        // drive at stack (I'm going for 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "08-0"));
+//
+//
+//        // drive away from stack slightly....................................................................... (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "09-0"));
+//
+//        //drive away to low (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "10-0"));
+//
+//        // rotate at low junction (angle = -135, direction = CW) (I have a 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "11-0"));
+//
+//        // driving forward / backwards to adjust (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "12-0"));
+//
+//        // counteract distance driven .......................................................................(I'm going for 3rd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "13-0"));
+//
+//        // rotate at stack (angle = -90, direction = CCW) (I'm going for 3rd cone)
+//        addState(new RotationState(robot, "Right State Auto", "14-0"));
+//
+//        // drive at stack (I'm going for 3rd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "15-0"));
+//
+//        // drive away from stack slightly................................................................... (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "16-0"));
+//
+//        //drive away to mid (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "17-0"));
+//
+//        // rotate at mid junction (angle = -135, direction = CW) (I have a 3rd and final cone)
+//        addState(new RotationState(robot, "Right State Auto", "18-0"));
+//
+//        // driving forward / backwards to adjust (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "19-0"));
+//
+//        // counteract distance driven.............................................................................. (I'm parking)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "20-0"));
+//
+//        // rotate at opposing alliance (angle = 0, direction = CCW) (I'm parking)
+//        addState(new RotationState(robot, "Right State Auto", "21-0"));
+//
+//        // Drive back one tile (I'm parking)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "22-0"));
+//
+//        // rotate towards stack side (-90 CW) (I'm parking)
+//        addState(new RotationState(robot, "Right State Auto", "23-0"));
+//
+//        // Choose Parking Spot (I'm parking)
+//        addState(new PathDecision(robot, "RightSideAutonomous", "24-0"));
+//
+//        // case 1 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-1"));
+//
+//        // case 2 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-2"));
+//
+//        // case 3 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-3"));
+//
+//        // rotate towards opposing alliance (angle = 0, direction = CCW) (I'm parking)
+//        addState(new RotationState(robot, "RightSideAutonomous", "25-0"));
+
+
+
+
+
+
+
+
+
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightTwoConeAutonomousEngine.java

@@ -1,23 +1,21 @@
-package org.timecrafters.Autonomous.Engines;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+package org.timecrafters.Phoenix.Autonomous.Engines;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverParkPlaceState;
-import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
-import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
-import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
-import org.timecrafters.Autonomous.States.PathDecision;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.ServoCameraRotate;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
+import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
+import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Phoenix.Autonomous.States.DriverStateWithOdometer;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentAngleState;
+import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentDistanceState;
+import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
+import org.timecrafters.Phoenix.Autonomous.States.RotationState;
+import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
+import org.timecrafters.Phoenix.Autonomous.States.TopArm;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
-@Autonomous (name = "Right 2 cone auto")
+//@Autonomous (name = "Right 2 cone auto")
 
 public class RightTwoConeAutonomousEngine extends CyberarmEngine {
     PhoenixBot1 robot;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/BottomArm.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class BottomArm extends CyberarmState {
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/CollectorDistanceState.java

@@ -1,10 +1,10 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class CollectorDistanceState extends CyberarmState {
 
@@ -12,7 +12,6 @@ public class CollectorDistanceState extends CyberarmState {
     private int traveledDistance;
     private int RampUpDistance;
     private int RampDownDistance;
-    private double drivePower;
     private double targetDrivePower;
     private double lastMeasuredTime;
     private double currentDistance;
@@ -24,6 +23,11 @@ public class CollectorDistanceState extends CyberarmState {
     private float collectTime;
     private double inRangeTime;
     private boolean stateDisabled;
+    private double distanceLimit;
+    private long maximumLookTime;
+    private long startOfSequencerTime;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
 
 
     public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -33,6 +37,8 @@ public class CollectorDistanceState extends CyberarmState {
         this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
         this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
         this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value();
+        this.distanceLimit = robot.configuration.variable(groupName, actionName, "distanceLimit").value();
+        this.maximumLookTime = robot.configuration.variable(groupName, actionName, "maximumLookTime").value();
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
 
 
@@ -40,25 +46,21 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void telemetry() {
+        engine.telemetry.addData("Time left", System.currentTimeMillis() - startOfSequencerTime);
+
         engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
         engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
-        engine.telemetry.addLine();
 
         engine.telemetry.addData("traveledDistance", traveledDistance);
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
 
-        engine.telemetry.addLine();
-
-        engine.telemetry.addData("drivePower", drivePower);
         engine.telemetry.addData("targetDrivePower", targetDrivePower);
 
         engine.telemetry.addLine();
-        engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
-        engine.telemetry.addData("Current Distance", currentDistance);
+        engine.telemetry.addData("Current Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
         engine.telemetry.addData("last Distance", LastDistanceRead);
         engine.telemetry.addLine();
 
@@ -75,25 +77,21 @@ public class CollectorDistanceState extends CyberarmState {
         robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
         robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
 
         robot.collectorLeft.setPower(1);
         robot.collectorRight.setPower(1);
 
         lastMeasuredTime = System.currentTimeMillis();
+        startOfSequencerTime = System.currentTimeMillis();
         LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM);
 
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
 
 
     }
@@ -102,13 +100,14 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void exec() {
-        if (stateDisabled){
+        if (stateDisabled || System.currentTimeMillis() - startOfSequencerTime > maximumLookTime) {
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
             robot.frontRightDrive.setPower(0);
             robot.frontLeftDrive.setPower(0);
             robot.backRightDrive.setPower(0);
             robot.backLeftDrive.setPower(0);
             setHasFinished(true);
-            return;
         }
 
         if (System.currentTimeMillis() - lastMeasuredTime > 150) {
@@ -136,65 +135,44 @@ public class CollectorDistanceState extends CyberarmState {
                 robot.backLeftDrive.setPower(0);
 
                 setHasFinished(true);
-
-                return;
             }
         }
 
-        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) {
+        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > distanceLimit) {
+
             double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
 
-            if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
-                // ramping up
-                drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.15;
-            } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
-                // ramping down
-                drivePower = ((delta / RampDownDistance) + 0.15);
-            } else {
-                // middle ground
-                drivePower = targetDrivePower;
-            }
+            robot.backLeftDrive.setPower(targetDrivePower);
+            robot.backRightDrive.setPower(targetDrivePower);
+            robot.frontLeftDrive.setPower(targetDrivePower);
+            robot.frontRightDrive.setPower(targetDrivePower);
 
-            if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
-                // This is limiting drive power to the targeted drive power
-                drivePower = targetDrivePower;
-            }
-
-            if (targetDrivePower < 0 && drivePower != targetDrivePower) {
-                drivePower = drivePower * -1;
-            }
-
-            if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
-                robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(drivePower);
-                robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(drivePower);
-            }
-    } else {
-        robot.frontRightDrive.setPower(0);
-        robot.frontLeftDrive.setPower(0);
-        robot.backRightDrive.setPower(0);
-        robot.backLeftDrive.setPower(0);
-
-        if (!inRange){
-            inRange = true;
-            inRangeTime = runTime();
         } else {
 
-            if (runTime() - inRangeTime >= collectTime){
-                robot.collectorRight.setPower(0);
-                robot.collectorLeft.setPower(0);
-                robot.frontRightDrive.setPower(0);
-                robot.frontLeftDrive.setPower(0);
-                robot.backRightDrive.setPower(0);
-                robot.backLeftDrive.setPower(0);
-                setHasFinished(true);
+            robot.frontRightDrive.setPower(0);
+            robot.frontLeftDrive.setPower(0);
+            robot.backRightDrive.setPower(0);
+            robot.backLeftDrive.setPower(0);
+
+            if (!inRange){
+                inRange = true;
+                inRangeTime = runTime();
+            } else {
+
+                if (runTime() - inRangeTime >= collectTime){
+                    robot.collectorRight.setPower(0);
+                    robot.collectorLeft.setPower(0);
+                    robot.frontRightDrive.setPower(0);
+                    robot.frontLeftDrive.setPower(0);
+                    robot.backRightDrive.setPower(0);
+                    robot.backLeftDrive.setPower(0);
+                    setHasFinished(true);
+                }
             }
-        }
+
+                }
 
             }
 
         }
 
-    }
-

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/CollectorState.java

@@ -1,8 +1,8 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class CollectorState extends CyberarmState {
 
@@ -52,8 +52,8 @@ public class CollectorState extends CyberarmState {
             }
 
         } else {
-//            robot.collectorLeft.setPower(0);
-//            robot.collectorRight.setPower(0);
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
             setHasFinished(true);
         }
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/ConeIdentification.java

@@ -1,8 +1,8 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 import java.util.List;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverParkPlaceState.java

@@ -0,0 +1,318 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class DriverParkPlaceState extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private String intendedPlacement;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
+    private double maximumTolerance;
+    private float direction;
+    private boolean targetAchieved = false;
+    public double startOfRampUpRight;
+    public double startOfRampDownRight;
+    public double startOfRampUpLeft;
+    public double startOfRampDownLeft;
+    public double endOfRampUpRight;
+    public double endOfRampDownRight;
+    public double endOfRampUpLeft;
+    public double endOfRampDownLeft;
+    public int driveStage;
+    public float currentAngle;
+    public double currentHorizontalEncoder;
+
+
+
+
+    public DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
+        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
+        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
+        this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
+        this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+    }
+    private double drivePower, targetDrivePower;
+    private int traveledDistance;
+
+    @Override
+    public void start() {
+        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+
+        if (targetDrivePower > 0) {
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
+
+        } else {
+
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
+
+        }
+
+        driveStage = 0;
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+        String placement = engine.blackboardGetString("parkPlace");
+        if (placement == null || !placement.equals(intendedPlacement)) {
+
+            setHasFinished(true);
+
+        } else {
+
+                double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
+                double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
+
+                // Driving Forward
+                if (targetDrivePower > 0 && driveStage == 0) {
+
+                    // ramping up
+                    if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
+                            (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
+
+                        drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    }
+
+                    // Driving Normal
+                    else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
+                            (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
+
+                        drivePower = targetDrivePower;
+
+                    }
+
+                    // Ramping down going forward
+                    else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) ||
+                            (LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) {
+                        drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (driveStage == 0){
+                        driveStage = 1;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
+                }
+
+                // Driving Backwards .................................................................................................................................Backwards
+                if (targetDrivePower < 0 && driveStage == 0) {
+
+                    // ramping up
+                    if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) ||
+                            (LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) {
+
+                        drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    }
+
+                    // Driving Normal
+                    else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) ||
+                            (LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) {
+
+                        drivePower = targetDrivePower;
+
+                    }
+
+                    // Ramping down going backward
+                    else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) ||
+                            (LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) {
+
+                        drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+                    } else if (driveStage == 0){
+                        driveStage = 1;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                    }
+
+                    // end of ramp down
+                }
+
+                // Forwards distance adjust...............................................................................................................................STAGE 1
+                if (driveStage == 1 && targetDrivePower > 0) {
+
+                    if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                            RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                        drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                            RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                        drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else {
+                        driveStage = 2;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
+                }
+
+                // backwards distance adjust
+                if (driveStage == 1 && targetDrivePower < 0) {
+
+                    if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                            RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                        drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                            RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                        drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else {
+                        driveStage = 2;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
+                }
+
+                if (driveStage == 0 || driveStage == 1) {
+                    robot.frontRightDrive.setPower(drivePower);
+                    robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+                    robot.backRightDrive.setPower(drivePower);
+                    robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+                }
+                // Heading adjustment
+                if (driveStage == 2 || driveStage == 4) {
+
+                    currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+                    if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
+
+                        robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                        robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                        robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER  );
+
+                    }
+                    else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
+
+                        robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                        robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                        robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                        robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+
+                    } else {
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                        driveStage ++;
+                    }
+                }
+
+                // ...........................................................................................................................................Strafe Adjustment
+                if ( driveStage == 3 ){
+
+                    currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
+                    if (currentHorizontalEncoder > 200){
+
+                        robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+
+                    }
+                    else if (currentHorizontalEncoder < -200){
+
+                        robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+
+                    } else {
+
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                        driveStage = 4;
+
+                    }
+
+                if (driveStage == 5) {
+                    setHasFinished(true);
+                }
+            } // placement equals if statement
+//            setHasFinished(true);
+        } // end of placement doesn't equal null
+
+    } // end of exec
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position", intendedPlacement);
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+    } // end of telemetry
+} // end of class

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverState.java

@@ -1,9 +1,9 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class DriverState extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverStateWithOdometer.java

@@ -0,0 +1,330 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class DriverStateWithOdometer extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private double RampUpDistance;
+    private double RampDownDistance;
+    private double maximumTolerance;
+    private float direction;
+    private boolean targetAchieved = false;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
+    public double startOfRampUpRight;
+    public double startOfRampDownRight;
+    public double startOfRampUpLeft;
+    public double startOfRampDownLeft;
+    public double endOfRampUpRight;
+    public double endOfRampDownRight;
+    public double endOfRampUpLeft;
+    public double endOfRampDownLeft;
+    public int driveStage;
+    public float currentAngle;
+    public double currentHorizontalEncoder;
+
+    public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
+        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
+        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
+        this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
+        this.direction = robot.configuration.variable(groupName, actionName, "direction").value();
+
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+    }
+
+    private double drivePower, targetDrivePower, traveledDistance;
+
+    @Override
+    public void start() {
+
+        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+
+
+        if (targetDrivePower > 0) {
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
+
+        } else {
+
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
+
+        }
+
+        driveStage = 0;
+
+    }
+
+    @Override
+    public void exec() {
+
+
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+
+        double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
+        double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
+
+        // Driving Forward
+        if (targetDrivePower > 0 && driveStage == 0) {
+
+            // ramping up
+            if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
+                    (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
+
+                drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+            }
+
+            // Driving Normal
+            else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
+                    (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
+
+                drivePower = targetDrivePower;
+
+            }
+
+            // Ramping down going forward
+            else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) ||
+                    (LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) {
+                drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else if (driveStage == 0){
+                driveStage = 1;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+            }
+        }
+
+        // Driving Backwards .................................................................................................................................Backwards
+        if (targetDrivePower < 0 && driveStage == 0) {
+
+            // ramping up
+            if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) ||
+                    (LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) {
+
+                drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+
+            }
+
+            // Driving Normal
+            else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) ||
+                    (LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) {
+
+                drivePower = targetDrivePower;
+
+            }
+
+            // Ramping down going backward
+            else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) ||
+                    (LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) {
+
+                drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                        (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+            } else if (driveStage == 0){
+                driveStage = 1;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+            }
+
+            // end of ramp down
+        }
+
+        // Forwards distance adjust...............................................................................................................................STAGE 1
+        if (driveStage == 1 && targetDrivePower > 0) {
+
+            if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                    RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                    RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else {
+                driveStage = 2;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+            }
+        }
+
+        // backwards distance adjust
+        if (driveStage == 1 && targetDrivePower < 0) {
+
+            if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                    RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                    RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+            } else {
+                driveStage = 2;
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+            }
+        }
+
+        if (driveStage == 0 || driveStage == 1) {
+            robot.frontRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+            robot.backRightDrive.setPower(drivePower);
+            robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
+        }
+        // Heading adjustment
+        if (driveStage == 2 || driveStage == 4) {
+
+            currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+            if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
+
+                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
+                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05);
+                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05  );
+                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+
+            }
+            else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
+
+                robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+                robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
+                robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
+                robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05);
+
+            } else {
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+                driveStage ++;
+            }
+        }
+
+        // ...........................................................................................................................................Strafe Adjustment
+        if ( driveStage == 3 ){
+
+            currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
+            if (currentHorizontalEncoder > 200){
+
+                robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+
+            }
+            else if (currentHorizontalEncoder < -200){
+
+                robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+
+            } else {
+
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+                driveStage = 4;
+
+            }
+        }
+
+        if (driveStage == 5) {
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void telemetry () {
+        engine.telemetry.addData("Stage", driveStage);
+
+        engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
+        engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
+        engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
+        engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight);
+        engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight);
+        engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight);
+        engine.telemetry.addData("endOfRampDownRight", endOfRampDownRight);
+        engine.telemetry.addData("startOfRampUpLeft", startOfRampUpLeft);
+        engine.telemetry.addData("endOfRampUpLeft", endOfRampUpLeft);
+        engine.telemetry.addData("startOfRampDownLeft", startOfRampDownLeft);
+        engine.telemetry.addData("endOfRampDownLeft", endOfRampDownLeft);
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
+        engine.telemetry.addData("maximumTolerance", maximumTolerance);
+        engine.telemetry.addData("imu 1 angle", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("Target Achieved", targetAchieved);
+
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/JunctionAllignmentAngleState.java

@@ -1,10 +1,11 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class JunctionAllignmentAngleState extends CyberarmState {
     private final boolean stateDisabled;
@@ -88,7 +89,7 @@ public class JunctionAllignmentAngleState extends CyberarmState {
     @Override
     public void exec() {
 
-        currentAngle = robot.imu.getAngularOrientation().firstAngle;
+        currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
 
         if (stateDisabled){

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/JunctionAllignmentDistanceState.java

@@ -1,10 +1,8 @@
-package org.timecrafters.Autonomous.States;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class JunctionAllignmentDistanceState extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/PathDecision.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class PathDecision extends CyberarmState {
     PhoenixBot1 robot;
@@ -17,6 +17,17 @@ public class PathDecision extends CyberarmState {
     @Override
     public void exec() {
         String placement = engine.blackboardGetString("parkPlace");
+
+        if (placement.equals("1")) {
+
+            addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 1"));
+        }
+
+        else if (placement.equals("3")){
+            addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 3"));
+
+        }
+
         setHasFinished(true);
         }
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/RotationState.java

@@ -0,0 +1,144 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class RotationState extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+
+    public RotationState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
+        this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
+        this.ClockWiseRotation = robot.configuration.variable(groupName, actionName, "ClockWiseRotation").value();
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+
+
+    }
+
+    private double drivePower;
+    private float targetRotation;
+    float CurrentRotation;
+    private String debugStatus = "?";
+    private double drivePowerVariable;
+    private double leftCompensator;
+    private double RightCompensator;
+    private boolean ClockWiseRotation;
+    private int RotationStage;
+    private double rotationDirection;
+    private long lastStepTime = 0;
+
+    @Override
+    public void start() {
+
+        leftCompensator = robot.OdometerEncoderLeft.getCurrentPosition();
+        RightCompensator = robot.OdometerEncoderRight.getCurrentPosition();
+
+        RotationStage = 0;
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        } //
+
+
+        CurrentRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+        if (RotationStage == 0) {
+
+            drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER;
+
+            if (ClockWiseRotation) {
+                rotationDirection = 1;
+            } else {
+                rotationDirection = -1;
+            }
+
+
+            robot.backLeftDrive.setPower(drivePowerVariable * rotationDirection);
+            robot.backRightDrive.setPower(-drivePowerVariable * rotationDirection);
+            robot.frontLeftDrive.setPower(drivePowerVariable * rotationDirection);
+            robot.frontRightDrive.setPower(-drivePowerVariable * rotationDirection);
+
+            if (Math.abs(Math.abs(CurrentRotation) - Math.abs(targetRotation)) <= robot.ROTATION_TOLERANCE &&
+                    (RotationStage == 0) &&
+                    (CurrentRotation - targetRotation <= robot.ROTATION_TOLERANCE)) {
+                RotationStage = 1;
+                lastStepTime = System.currentTimeMillis();
+            }
+        }
+
+        if (RotationStage == 1) {
+            robot.backLeftDrive.setPower(0);
+            robot.backRightDrive.setPower(0);
+            robot.frontLeftDrive.setPower(0);
+            robot.frontRightDrive.setPower(0);
+            if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
+                RotationStage = 2;
+            }
+        }
+
+        if (RotationStage == 2) {
+
+            if (CurrentRotation - targetRotation > robot.ROTATION_TOLERANCE) {
+                // CW
+
+                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+
+                lastStepTime = System.currentTimeMillis();
+
+            } else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) {
+                // CCW
+
+                robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+
+                lastStepTime = System.currentTimeMillis();
+
+            } else {
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+                if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
+                    RotationStage = 3;
+                }
+            }
+        }
+
+        if (RotationStage == 3) {
+            RotationStage ++;
+            setHasFinished(true);
+            }
+        }
+
+
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("DEBUG Status", debugStatus);
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("Robot IMU Rotation", CurrentRotation);
+        engine.telemetry.addData("Robot Target Rotation", targetRotation);
+        engine.telemetry.addData("Drive Power", drivePowerVariable);
+        engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
+        engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
+        engine.telemetry.addData("RotationStage", RotationStage);
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/ServoCameraRotate.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class ServoCameraRotate extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArm.java

@@ -1,7 +1,7 @@
-package org.timecrafters.Autonomous.States;
+package org.timecrafters.Phoenix.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class TopArm extends CyberarmState {
 

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmResetState.java

@@ -0,0 +1,48 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TopArmResetState extends CyberarmState {
+    private final PhoenixBot1 robot;
+    private double targetPower;
+    private int targetPosition;
+    private int tolerance;
+    private long lastMeasuredTime;
+    private long pausingTime;
+
+
+    public TopArmResetState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value();
+        this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
+        this.pausingTime = robot.configuration.variable(groupName, actionName, "pausingTime").value();
+    }
+
+    @Override
+    public void start() {
+        robot.ArmMotor.setTargetPosition(targetPosition);
+        robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ArmMotor.setPower(targetPower);
+        lastMeasuredTime = System.currentTimeMillis();
+
+    }
+
+    @Override
+    public void exec() {
+
+        if (System.currentTimeMillis() - lastMeasuredTime > pausingTime) {
+
+            robot.ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            robot.ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            robot.ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            robot.ArmMotor.setTargetPosition(0);
+            robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            robot.ArmMotor.setPower(0.2);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmv2.java

@@ -0,0 +1,37 @@
+package org.timecrafters.Phoenix.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TopArmv2 extends CyberarmState {
+    private final PhoenixBot1 robot;
+    private double targetPower;
+    private int targetPosition;
+    private int tolerance;
+
+
+    public TopArmv2(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value();
+        this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
+    }
+
+    @Override
+    public void start() {
+        robot.ArmMotor.setTargetPosition(targetPosition);
+        robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ArmMotor.setPower(targetPower);
+
+    }
+
+    @Override
+    public void exec() {
+
+        if (robot.ArmMotor.getTargetPosition() == targetPosition) {
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/PhoenixBot1.java

@@ -1,11 +1,14 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix;
 
 import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
@@ -22,11 +25,19 @@ import org.timecrafters.minibots.cyberarm.chiron.states.autonomous.Arm;
 public class PhoenixBot1 {
 
     private static final float mmPerInch        = 25.4f;
-    public static final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public static double WHEEL_CIRCUMFERENCE;
     public static final int COUNTS_PER_REVOLUTION = 8192;
     public static double leftCompensatorGlobal;
     public static double RightCompensatorGlobal;
     public double VEER_COMPENSATION_DBL; // some place around 1, .99 is 1% power reduction
+    public double DRIVETRAIN_MINIMUM_POWER;
+    public double ROTATION_MINIMUM_POWER;
+    public double STRAFE_MINIMUM_POWER;
+    public double DRIVE_TOLERANCE;
+    public double ROTATION_TOLERANCE;
+    public long PAUSE_ON_ROTATION;
+    public double DISTANCE_MULTIPLIER;
+    public double CAMERA_INITiAL_POS;
 
 //    private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
     private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -51,11 +62,13 @@ public class PhoenixBot1 {
 
     public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance;
 
-        public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor;
+        public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal;
+
+        public DcMotorEx ArmMotor;
 
         public CRServo collectorLeft, collectorRight;
 
-         public BNO055IMU imu;
+         public IMU imu;
 
          public TimeCraftersConfiguration configuration;
 
@@ -84,6 +97,15 @@ public class PhoenixBot1 {
 
         public void initConstants(){
             VEER_COMPENSATION_DBL = configuration.variable("Robot", "Tuning", "VEER_COMPENSATION_DBL").value();
+            DRIVETRAIN_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "DRIVETRAIN_MINIMUM_POWER").value();
+            ROTATION_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "ROTATION_MINIMUM_POWER").value();
+            STRAFE_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "STRAFE_MINIMUM_POWER").value();
+            DRIVE_TOLERANCE = configuration.variable("Robot", "Tuning", "DRIVE_TOLERANCE").value();
+            ROTATION_TOLERANCE = configuration.variable("Robot", "Tuning", "ROTATION_TOLERANCE").value();
+            PAUSE_ON_ROTATION = configuration.variable("Robot", "Tuning", "PAUSE_ON_ROTATION").value();
+            WHEEL_CIRCUMFERENCE = configuration.variable("Robot", "Tuning", "WHEEL_CIRCUMFERENCE").value();
+            DISTANCE_MULTIPLIER = configuration.variable("Robot", "Tuning", "DISTANCE_MULTIPLIER").value();
+            CAMERA_INITiAL_POS = configuration.variable("Robot", "Tuning", "CAMERA_INITiAL_POS").value();
         }
 
     private void initVuforia(){
@@ -121,17 +143,20 @@ public class PhoenixBot1 {
             leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
             rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance");
 
-            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+            IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                    RevHubOrientationOnRobot.UsbFacingDirection.UP
+            ));
 
-            parameters.mode = BNO055IMU.SensorMode.IMU;
-            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
-            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-            parameters.loggingEnabled = false;
+//            parameters.mode = BNO055IMU.SensorMode.IMU;
+//            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+//            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+//            parameters.loggingEnabled = false;
 
-            this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
+            this.imu = engine.hardwareMap.get(IMU.class, "imu");
             imu.initialize(parameters);
 
-            imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
+//            imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
 
             configuration = new TimeCraftersConfiguration("Phoenix");
 //            AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit");
@@ -160,7 +185,7 @@ public class PhoenixBot1 {
             LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight");
 //            HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft");
 //            HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight");
-            ArmMotor = engine.hardwareMap.dcMotor.get("ArmMotor");
+            ArmMotor = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor");
 
             //motors direction and encoders
 
@@ -212,10 +237,10 @@ public class PhoenixBot1 {
 //            HighRiserRight.setDirection(Servo.Direction.FORWARD);
             LowRiserLeft.setDirection(Servo.Direction.FORWARD);
             LowRiserRight.setDirection(Servo.Direction.REVERSE);
-//            ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-//            ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-//            ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-//            ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+            ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
             CameraServo.setDirection(Servo.Direction.FORWARD);
 
@@ -224,7 +249,9 @@ public class PhoenixBot1 {
 //            HighRiserLeft.setPosition(0.40);
 //            HighRiserRight.setPosition(0.40);
 
-            CameraServo.setPosition(0.775);
+            CameraServo.setPosition(CAMERA_INITiAL_POS);
+
+
 
         }
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/CameraTestEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/DriveDebugEngine.java

@@ -1,7 +1,7 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class DriveDebugEngine extends CyberarmEngine {
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/DynamicSetupEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixTeleOP.java

@@ -1,12 +1,11 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.PhoenixTeleOPState;
-import org.timecrafters.TeleOp.states.TeleOPArmDriver;
-import org.timecrafters.TeleOp.states.TeleOPTankDriver;
+import org.timecrafters.Phoenix.PhoenixBot1;
+import org.timecrafters.Phoenix.TeleOp.states.TeleOPArmDriver;
+import org.timecrafters.Phoenix.TeleOp.states.TeleOPTankDriver;
 
 @TeleOp (name = "APhoenixTeleOP")
 
@@ -18,7 +17,6 @@ public class PhoenixTeleOP extends CyberarmEngine {
     public void setup() {
 
         robot = new PhoenixBot1(this);
-//        addState(new PhoenixTeleOPState(robot));
         addState(new TeleOPArmDriver(robot));
         addParallelStateToLastState(new TeleOPTankDriver(robot));
     }

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixWingEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.CRServo;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SodiEngine.java

@@ -1,10 +1,10 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.LaserState;
+import org.timecrafters.Phoenix.TeleOp.states.LaserState;
 
 @Disabled
 @TeleOp(name = "Wheel")

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SpeedrunEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -6,9 +6,9 @@ import com.qualcomm.robotcore.hardware.Gamepad;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.cyberarm.engine.V2.GamepadChecker;
+import org.timecrafters.Phoenix.TeleOp.states.TeleOPSpeedrunState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.TeleOPSpeedrunState;
 
 @Disabled
 @TeleOp (name = "Speedrun Engine")

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SterringDriveEngine.java

@@ -1,12 +1,11 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Phoenix.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.LaserState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.SteeringDriveExperiment;
+import org.timecrafters.Phoenix.TeleOp.states.SteeringDriveExperiment;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 @Disabled
 @TeleOp(name = "Steering Drive Test")

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/CameraTestCommon.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.firstinspires.ftc.robotcore.external.tfod.Recognition;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/CameraTestState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/DynamicSetupState.java

@@ -1,7 +1,7 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.engine.DynamicSetupEngine;
+import org.timecrafters.Phoenix.TeleOp.engine.DynamicSetupEngine;
 
 public class DynamicSetupState extends CyberarmState {
     private long delay;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserServoHeight.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.robotcore.hardware.Servo;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPState.java

@@ -1,16 +1,16 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import android.annotation.SuppressLint;
-import android.widget.Switch;
 
-import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.cyberarm.engine.V2.GamepadChecker;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class PhoenixTeleOPState extends CyberarmState {
 
@@ -52,7 +52,7 @@ public class PhoenixTeleOPState extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
         engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
-        engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
+        engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
         engine.telemetry.addData("Drive Power", drivePower);
         engine.telemetry.addData("Delta Rotation", DeltaRotation);
         engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM));
@@ -131,7 +131,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.dpad_right) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 90;
             CalculateDeltaRotation();
             if (RobotRotation > -90 && RobotRotation < 89) {//CCW
@@ -158,7 +158,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.dpad_left) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = -90;
             CalculateDeltaRotation();
             if (RobotRotation > -89 && RobotRotation <= 90) {//CW
@@ -185,7 +185,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.y) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 180;
             CalculateDeltaRotation();
             if (RobotRotation < 0 && RobotRotation > -179) {
@@ -210,7 +210,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.a && !engine.gamepad1.start) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 0;
             CalculateDeltaRotation();
             if (RobotRotation < -1) {
@@ -237,7 +237,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.x) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = -45;
             CalculateDeltaRotation();
             if (RobotRotation < -46 || RobotRotation > 135) {//CCW
@@ -264,7 +264,7 @@ public class PhoenixTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad1.b) {
-            RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+            RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
             RotationTarget = 45;
             CalculateDeltaRotation();
             if (RobotRotation > -135 && RobotRotation < 44) {//CCW
@@ -470,24 +470,10 @@ public class PhoenixTeleOPState extends CyberarmState {
     @Override
     public void buttonUp(Gamepad gamepad, String button) {
         if (engine.gamepad1 == gamepad && button.equals("start")) {
-            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
-            parameters.mode = BNO055IMU.SensorMode.IMU;
-            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
-            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-            parameters.loggingEnabled = false;
-
-            robot.imu.initialize(parameters);
+            robot.imu.resetYaw();
         }
     }
-//    public double downSensor() {
-//        double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
-//        Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
-//        Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
-//        return Distance;
+
 
     }

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPv2.java

@@ -1,6 +1,7 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 public class PhoenixTeleOPv2 extends CyberarmState {
     private double drivePower = 1;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixWingState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SodiLEDState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SodiState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SteeringDriveExperiment.java

@@ -1,6 +1,7 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 
 
 public class SteeringDriveExperiment extends CyberarmState {

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPArmDriver.java

@@ -0,0 +1,234 @@
+package org.timecrafters.Phoenix.TeleOp.states;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.cyberarm.engine.V2.GamepadChecker;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TeleOPArmDriver extends CyberarmState {
+    private final PhoenixBot1 robot;
+    private long lastStepTime = 0, Spirit;
+    private int CyclingArmUpAndDown = 0;
+    private GamepadChecker gamepad2Checker;
+    private int Opportunity, JunctionHeight, Ingenuity;
+    private double drivePower, armPower;
+    private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
+    private double servoCollect = 0.45; //Low servos, A button
+    //    private double servoCollectHigh = 0.40; //High servos, A button
+    private double servoLow = 0.45; //Low servos, X button
+    //    private double servoLowHigh = 0.75; //High servos, X button
+    private double servoMed = 0.45; //Low servos, B button
+    //    private double servoMedHigh = 0.9; //High servos, B button
+    private double servoHigh = 0.8; //Low servos, Y button
+    //    private double servoHighHigh = 0.9; //High servos, Y button
+    private double ArmNeededPower;
+    private int armMotorCollect = -100;
+    private int armMotorLow = 280;
+    private int armMotorMed = 430;
+    private int armMotorHigh = 510;
+    private int ArmMotorStepSize = 2;
+    private int TargetPosition = 0, OverrideTarget = -64;
+
+    public TeleOPArmDriver(PhoenixBot1 robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Arm Driver:");
+        engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower());
+        engine.telemetry.addData("Arm Motor Encoder Position", robot.ArmMotor.getCurrentPosition());
+        engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
+        engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
+        engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition());
+        engine.telemetry.addData("Target Junction Height", JunctionHeight);
+    }
+
+    @Override
+    public void init() {
+        robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
+        robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
+        robot.LowRiserLeft.setPosition(0.45);
+        robot.LowRiserRight.setPosition(0.45);
+        robot.ArmMotor.setTargetPosition(0);
+        robot.ArmMotor.setPower(0.5);
+        robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ArmMotor.setTargetPositionTolerance(5);
+        armPower = 0.4;
+
+        JunctionHeight = 0;
+    }
+
+    @Override
+    public void exec() {
+
+        double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - TargetPosition) / 400.0 + 0.1;
+
+
+//        ArmNeededPower =  Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
+//        robot.ArmMotor.setTargetPosition(TargetPosition);
+//        robot.ArmMotor.setPower(armPower);
+
+        if (engine.gamepad2.y) {
+            JunctionHeight = 4;
+            TargetPosition = armMotorHigh;
+        }
+        if (engine.gamepad2.b) {
+            JunctionHeight = 3;
+            TargetPosition = armMotorMed;
+        }
+        if (engine.gamepad2.x) {
+            JunctionHeight = 2;
+            TargetPosition = armMotorLow;
+        }
+        if (engine.gamepad2.a) {
+            JunctionHeight = 1;
+            TargetPosition = armMotorCollect;
+        }
+
+        if (JunctionHeight == 4) {
+            if (robot.LowRiserLeft.getPosition() <= servoHigh && robot.ArmMotor.getTargetPosition() != armMotorHigh) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorHigh);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoHigh) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorHigh && robot.LowRiserLeft.getPosition() == servoHigh) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (JunctionHeight == 3) {
+            if (robot.LowRiserLeft.getPosition() <= servoMed && robot.ArmMotor.getTargetPosition() != armMotorMed) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorMed);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoMed) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
+                }
+            } else if (robot.LowRiserLeft.getPosition() > servoMed) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorMed && robot.LowRiserLeft.getPosition() == servoMed) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (JunctionHeight == 2) {
+            if (robot.LowRiserLeft.getPosition() <= servoLow && robot.ArmMotor.getTargetPosition() != armMotorLow) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorLow);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.ArmMotor.getCurrentPosition() >= armMotorLow && robot.LowRiserLeft.getPosition() < servoLow) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
+                }
+            } else if (robot.LowRiserLeft.getPosition() > servoLow) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorLow && robot.LowRiserLeft.getPosition() == servoLow) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (JunctionHeight == 1) {
+            if (robot.LowRiserLeft.getPosition() <= servoCollect && robot.ArmMotor.getTargetPosition() != armMotorCollect) {
+                robot.ArmMotor.setPower(0);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(armMotorCollect);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+            if (robot.LowRiserLeft.getPosition() > servoCollect) {
+                if (System.currentTimeMillis() - lastStepTime >= 100) {
+                    lastStepTime = System.currentTimeMillis();
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
+                }
+            } else if (robot.ArmMotor.getTargetPosition() == armMotorCollect && robot.LowRiserLeft.getPosition() == servoCollect) {
+                JunctionHeight = 0;
+            }
+        }
+
+        if (engine.gamepad2.left_bumper) {
+            if (robot.ArmMotor.getTargetPosition() != OverrideTarget) {
+                OverrideTarget = robot.ArmMotor.getCurrentPosition() - 80;
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(OverrideTarget);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+        }
+
+        if (engine.gamepad2.right_bumper) {
+            if (robot.ArmMotor.getTargetPosition() != OverrideTarget) {
+                OverrideTarget = robot.ArmMotor.getCurrentPosition() + 80;
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ArmMotor.setPower(armPower);
+                robot.ArmMotor.setTargetPosition(OverrideTarget);
+                robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+        }
+
+        if (engine.gamepad2.dpad_left) {
+            robot.collectorRight.setPower(-1);
+            robot.collectorLeft.setPower(-1);
+        } else if (engine.gamepad2.dpad_right) {
+            robot.collectorLeft.setPower(1);
+            robot.collectorRight.setPower(1);
+        } else {
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
+        }
+
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (engine.gamepad2 == gamepad) {
+            if (button.equals("right_bumper")) {
+                OverrideTarget = -64;
+            } else if (button.equals("left_bumper")) {
+                OverrideTarget = -64;
+            }
+        }
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (engine.gamepad2 == gamepad) {
+            if (button.equals("right_bumper")) {
+
+            } else if (button.equals("left_bumper")) {
+
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPSpeedrunState.java

@@ -1,8 +1,9 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Phoenix.TeleOp.states;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Phoenix.PhoenixBot1;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
 public class TeleOPSpeedrunState extends CyberarmState {

TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPTankDriver.java

@@ -0,0 +1,144 @@
+package org.timecrafters.Phoenix.TeleOp.states;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.cyberarm.engine.V2.GamepadChecker;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.Phoenix.PhoenixBot1;
+
+public class TeleOPTankDriver extends CyberarmState {
+
+    private final PhoenixBot1 robot;
+    private long lastStepTime = 0;
+    private double drivePower = 0.3;
+    private double RobotRotation;
+    private double RotationTarget, DeltaRotation;
+    private double MinimalPower = 0.2;
+    private int DeltaOdometerR, Spirit;
+    private GamepadChecker gamepad1Checker;
+
+    public TeleOPTankDriver(PhoenixBot1 robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Tank Driver");
+        engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("Drive Power", drivePower);
+        engine.telemetry.addData("Delta Rotation", DeltaRotation);
+    }
+
+    @Override
+    public void init() {
+
+    }
+
+    @Override
+    public void exec() {
+
+        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative //ORLY?
+        double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
+        double rx = engine.gamepad1.right_stick_x;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+        double frontLeftPower = (y + x + rx) / denominator;
+        double backLeftPower = (y - x + rx) / denominator;
+        double frontRightPower = (y - x - rx) / denominator;
+        double backRightPower = (y + x - rx) / denominator;
+
+
+        double heading = -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+        double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+        double rotY = x * Math.sin(heading) + y * Math.cos(heading);
+
+        frontLeftPower = (rotY + rotX + rx) / denominator;
+        backLeftPower = (rotY - rotX + rx) / denominator;
+        frontRightPower = (rotY - rotX - rx) / denominator;
+        backRightPower = (rotY + rotX - rx) / denominator;
+
+        robot.backLeftDrive.setPower(backLeftPower * drivePower);
+        robot.backRightDrive.setPower(backRightPower * drivePower);
+        robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
+        robot.frontRightDrive.setPower(frontRightPower * drivePower);
+
+
+
+//        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
+//            drivePower = engine.gamepad1.left_stick_y;
+//            robot.backLeftDrive.setPower(drivePower);
+//            robot.backRightDrive.setPower(drivePower);
+//            robot.frontLeftDrive.setPower(drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+//
+//        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) {
+//            drivePower = engine.gamepad1.left_stick_x;
+//            robot.backLeftDrive.setPower(drivePower);
+//            robot.backRightDrive.setPower(-drivePower);
+//            robot.frontLeftDrive.setPower(-drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+//
+//        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
+//            robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
+//            robot.backLeftDrive.setPower(backLeftPower * drivePower);
+//            robot.frontRightDrive.setPower(frontRightPower * drivePower);
+//            robot.backRightDrive.setPower(backRightPower * drivePower);
+//        }
+//
+//        if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
+//            drivePower = engine.gamepad1.right_stick_x;
+//            robot.backLeftDrive.setPower(-drivePower);
+//            robot.backRightDrive.setPower(drivePower);
+//            robot.frontLeftDrive.setPower(-drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+//
+//        if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) {
+//            drivePower = 0;
+//            robot.backLeftDrive.setPower(drivePower);
+//            robot.backRightDrive.setPower(drivePower);
+//            robot.frontLeftDrive.setPower(drivePower);
+//            robot.frontRightDrive.setPower(drivePower);
+//        }
+
+
+
+    }
+
+    public void CalculateDeltaRotation() {
+        if (RotationTarget >= 0 && RobotRotation >= 0) {
+            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
+        } else if (RotationTarget <= 0 && RobotRotation <= 0) {
+            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
+        } else if (RotationTarget >= 0 && RobotRotation <= 0) {
+            DeltaRotation = Math.abs(RotationTarget + RobotRotation);
+        } else if (RotationTarget <= 0 && RobotRotation >= 0) {
+            DeltaRotation = Math.abs(RobotRotation + RotationTarget);
+        }
+    }
+
+    public void getDeltaOdometerR() {
+        Spirit = robot.OdometerEncoderRight.getCurrentPosition();
+        if (System.currentTimeMillis() - lastStepTime >= 1000) {
+            lastStepTime = System.currentTimeMillis();
+            DeltaOdometerR = robot.OdometerEncoderRight.getCurrentPosition() - Spirit;
+        }
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (engine.gamepad1 == gamepad && button.equals("start")) {
+
+            robot.imu.resetYaw();
+        }
+        if (engine.gamepad1 == gamepad && button.equals("y")) {
+            drivePower = 1;
+        }
+        if (engine.gamepad1 == gamepad && button.equals("a")) {
+            drivePower = 0.5;
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java

@@ -1,228 +0,0 @@
-package org.timecrafters.TeleOp.states;
-
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.cyberarm.engine.V2.GamepadChecker;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-
-public class TeleOPArmDriver extends CyberarmState {
-    private final PhoenixBot1 robot;
-    private long lastStepTime = 0;
-    private int CyclingArmUpAndDown = 0;
-    private GamepadChecker gamepad2Checker;
-    private int Opportunity, Endeavour, Peanut;
-    private double drivePower, armPower;
-    private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
-    private double servoCollectLow = 0.40; //Low servos, A button
-//    private double servoCollectHigh = 0.40; //High servos, A button
-    private double servoLowLow = 0.5; //Low servos, X button
-//    private double servoLowHigh = 0.75; //High servos, X button
-    private double servoMedLow = 0.5; //Low servos, B button
-//    private double servoMedHigh = 0.9; //High servos, B button
-    private double servoHighLow = 0.8; //Low servos, Y button
-//    private double servoHighHigh = 0.9; //High servos, Y button
-    private double ArmNeededPower;
-    private int armMotorCollect = 0;
-    private int armMotorLow = 240;
-    private int armMotorMed = 380;
-    private int armMotorHigh = 463;
-    private int ArmMotorStepSize = 2;
-
-    public TeleOPArmDriver(PhoenixBot1 robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addLine("Arm Driver:");
-        engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower());
-        engine.telemetry.addData("Arm Motor Encoder Position", robot.ArmMotor.getCurrentPosition());
-        engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
-        engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
-        engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition());
-        engine.telemetry.addData("Target (Endeavour)", Endeavour);
-    }
-
-    @Override
-    public void init() {
-        robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
-        robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
-        robot.LowRiserLeft.setPosition(0.45);
-        robot.LowRiserRight.setPosition(0.45);
-        robot.ArmMotor.setTargetPosition(0);
-        robot.ArmMotor.setPower(0.25);
-        Opportunity = 0;
-        Endeavour = 0;
-    }
-
-
-
-@Override
-public void exec() {
-
-    if (Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) > 46) {
-        ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
-        armPower = ArmNeededPower;
-        robot.ArmMotor.setPower(armPower);
-    }
-
-        if (engine.gamepad2.y) {
-            Endeavour = 4;
-        }
-        if (engine.gamepad2.b) {
-            Endeavour = 3;
-        }
-        if (engine.gamepad2.x) {
-            Endeavour = 2;
-        }
-        if (engine.gamepad2.a) {
-            Endeavour = 1;
-        }
-
-        if (Endeavour == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
-            lastStepTime = System.currentTimeMillis();
-            robot.ArmMotor.setTargetPosition(armMotorHigh);
-            if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ {
-                robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
-            }
-            if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
-                robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-                robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-            }
-            if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh &&
-                    robot.LowRiserLeft.getPosition() >= servoHighLow) {
-                Endeavour = 0;
-            }
-        }
-
-        if (Endeavour == 3) {
-            robot.ArmMotor.setTargetPosition(armMotorMed);
-            if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() < servoMedLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() <= servoMedLow &&
-                    robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() < servoMedLow &&
-                    robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
-                    robot.LowRiserLeft.getPosition() <= servoMedLow &&
-                    robot.ArmMotor.getCurrentPosition() >= armMotorMed) {
-                Endeavour = 0;
-            }
-        }
-
-        if (Endeavour == 2) {
-            robot.ArmMotor.setTargetPosition(armMotorLow);
-            if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
-                    robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
-                    robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
-                    robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
-                }
-            }
-            if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
-                    robot.LowRiserLeft.getPosition() <= servoLowLow &&
-                    robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
-                Endeavour = 0;
-            }
-        }
-
-        if (Endeavour == 1) {
-            robot.ArmMotor.setTargetPosition(armMotorCollect);
-            if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
-                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
-                }
-            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
-                    robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
-                if (System.currentTimeMillis() - lastStepTime >= 100) {
-                    lastStepTime = System.currentTimeMillis();
-                    robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
-                }
-            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
-                    robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
-                Endeavour = 0;
-            }
-        }
-
-        if (engine.gamepad2.dpad_left && Peanut != 1) {
-            Peanut = 1;
-        }
-
-        if (engine.gamepad2.dpad_right && Peanut != 2) {
-            Peanut = 2;
-        }
-
-        if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) {
-            robot.collectorLeft.setPower(0);
-            robot.collectorRight.setPower(0);
-            Peanut = 0;
-        }
-
-        if (Peanut == 1) {
-            robot.collectorRight.setPower(1);
-            robot.collectorLeft.setPower(-1);
-        }
-
-        if (Peanut == 2) {
-            robot.collectorLeft.setPower(1);
-            robot.collectorRight.setPower(-1);
-        }
-
-        if (engine.gamepad2.right_trigger > 0.1) {
-
-        } else if (engine.gamepad2.left_trigger > 0.1) {
-
-        } else if (engine.gamepad2.right_trigger == 0 && engine.gamepad2.left_trigger == 0 && Endeavour == 0) {
-
-        }
-
-    }
-}

TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java

@@ -1,125 +0,0 @@
-package org.timecrafters.TeleOp.states;
-
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.cyberarm.engine.V2.GamepadChecker;
-
-public class TeleOPTankDriver extends CyberarmState {
-
-    private final PhoenixBot1 robot;
-    private long lastStepTime = 0;
-    private double drivePower = 0.3;
-    private double RobotRotation;
-    private double RotationTarget, DeltaRotation;
-    private double MinimalPower = 0.2;
-    private int DeltaOdometerR, Endeavour, Spirit;
-    private GamepadChecker gamepad1Checker;
-
-    public TeleOPTankDriver(PhoenixBot1 robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addLine("Tank Driver");
-        engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
-        engine.telemetry.addData("Drive Power", drivePower);
-        engine.telemetry.addData("Delta Rotation", DeltaRotation);
-    }
-
-    @Override
-    public void init() {
-    }
-
-    @Override
-    public void exec() {
-
-        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
-        double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
-        double rx = engine.gamepad1.right_stick_x;
-        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-        double frontLeftPower = (y + x + rx) / denominator;
-        double backLeftPower = (y - x + rx) / denominator;
-        double frontRightPower = (y - x - rx) / denominator;
-        double backRightPower = (y + x - rx) / denominator;
-
-        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
-            drivePower = engine.gamepad1.left_stick_y;
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-        }
-
-        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) {
-            drivePower = engine.gamepad1.left_stick_x;
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(-drivePower);
-            robot.frontLeftDrive.setPower(-drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-        }
-
-        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
-            robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
-            robot.backLeftDrive.setPower(backLeftPower * drivePower);
-            robot.frontRightDrive.setPower(frontRightPower * drivePower);
-            robot.backRightDrive.setPower(backRightPower * drivePower);
-        }
-
-        if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
-            drivePower = engine.gamepad1.right_stick_x;
-            robot.backLeftDrive.setPower(-drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(-drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-        }
-
-        if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) {
-            drivePower = 0;
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-        }
-
-
-
-    }
-
-    public void CalculateDeltaRotation() {
-        if (RotationTarget >= 0 && RobotRotation >= 0) {
-            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
-        } else if (RotationTarget <= 0 && RobotRotation <= 0) {
-            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
-        } else if (RotationTarget >= 0 && RobotRotation <= 0) {
-            DeltaRotation = Math.abs(RotationTarget + RobotRotation);
-        } else if (RotationTarget <= 0 && RobotRotation >= 0) {
-            DeltaRotation = Math.abs(RobotRotation + RotationTarget);
-        }
-    }
-
-    public void getDeltaOdometerR() {
-        Spirit = robot.OdometerEncoderRight.getCurrentPosition();
-        if (System.currentTimeMillis() - lastStepTime >= 1000) {
-            lastStepTime = System.currentTimeMillis();
-            DeltaOdometerR = robot.OdometerEncoderRight.getCurrentPosition() - Spirit;
-        }
-    }
-
-    @Override
-    public void buttonUp(Gamepad gamepad, String button) {
-        if (engine.gamepad1 == gamepad && button.equals("start")) {
-            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-
-            parameters.mode = BNO055IMU.SensorMode.IMU;
-            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
-            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-            parameters.loggingEnabled = false;
-
-            robot.imu.initialize(parameters);
-        }
-    }
-
-}

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

@@ -10,6 +10,7 @@ import org.timecrafters.minibots.cyberarm.chiron.Robot;
 public class DrivetrainDriverControl extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
+    private final double robotCentricRotation;
 
     private Gamepad controller;
 
@@ -23,6 +24,8 @@ public class DrivetrainDriverControl extends CyberarmState {
         this.actionName = actionName;
 
         this.controller = engine.gamepad1;
+
+        this.robotCentricRotation = robot.tuningConfig("robot_centric_rotation").value();
     }
 
     @Override
@@ -50,6 +53,13 @@ public class DrivetrainDriverControl extends CyberarmState {
         double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
         double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x);
 
+        if (!fieldCentricControl) {
+            Vector2d v = new Vector2d(x, y).rotated(robotCentricRotation);
+
+            x = v.getX();
+            y = v.getY();
+        }
+
         // Improve control?
         if (y < 0) {
             y = -Math.sqrt(-y);
@@ -160,6 +170,10 @@ public class DrivetrainDriverControl extends CyberarmState {
         if (button.equals("right_stick_button")) {
             robotSlowMode = !robotSlowMode;
         }
+
+        if (button.equals("left_stick_button") && robot.hardwareFault) {
+            robot.imu.resetYaw();
+        }
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java

@@ -0,0 +1,86 @@
+package org.timecrafters.minibots.cyberarm.phoenix;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class PositionalServoController {
+    final private Servo servo;
+    final private double targetDegreesPerSecond, servoDegreesPerSecond, servoRangeInDegrees;
+
+    private double lastEstimatedPosition, estimatedPosition, workingPosition, targetPosition;
+    private long lastUpdatedAt;
+    private boolean travelling = false;
+
+    public PositionalServoController(Servo servo, double targetDegreesPerSecond, double servoDegreesPerSecond, double servoRangeInDegrees) {
+        this.servo = servo;
+        this.targetDegreesPerSecond = targetDegreesPerSecond;
+        this.servoDegreesPerSecond = servoDegreesPerSecond;
+        this.servoRangeInDegrees = servoRangeInDegrees;
+
+        this.workingPosition = servo.getPosition();
+        this.estimatedPosition = workingPosition;
+        this.lastEstimatedPosition = estimatedPosition;
+        this.lastUpdatedAt = System.currentTimeMillis();
+    }
+
+    public Servo getServo() {
+        return servo;
+    }
+
+    public void update() {
+        double error = targetPosition - estimatedPosition;
+        double delta = (System.currentTimeMillis() - lastUpdatedAt) / 1000.0;
+        double estimatedTravel = servoDegreesPerSecond * delta;
+        double targetTravel = targetDegreesPerSecond * delta;
+
+        if (targetTravel < estimatedTravel) {
+            estimatedTravel = targetTravel;
+        }
+
+        if (travelling) {
+            this.lastEstimatedPosition = estimatedPosition;
+
+            if (error < 0.0) {
+                this.estimatedPosition -= estimatedTravel;
+            } else {
+                this.estimatedPosition += estimatedTravel;
+            }
+        }
+
+        if (error < 0.0 - estimatedTravel) {
+            workingPosition -= estimatedTravel;
+            travelling = true;
+        } else if (error > 0.0 + estimatedTravel) {
+            workingPosition += estimatedTravel;
+            travelling = true;
+        } else {
+            workingPosition = targetPosition;
+            travelling = false;
+        }
+
+        servo.setPosition(workingPosition);
+        this.lastUpdatedAt = System.currentTimeMillis();
+    }
+
+    public void setTargetPosition(double targetPosition) {
+        this.targetPosition = targetPosition;
+    }
+
+    public double getEstimatedPosition() {
+        return estimatedPosition;
+    }
+
+    public double getWorkingPosition() { return workingPosition; }
+
+    public double getTargetPosition() { return targetPosition; }
+
+    public double getEstimatedAngle() {
+        return estimatedPosition * servoRangeInDegrees;
+    }
+
+    public double getError() { return targetPosition - estimatedPosition; }
+
+    private double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+}

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

@@ -0,0 +1,724 @@
+package org.timecrafters.minibots.cyberarm.phoenix;
+
+import android.annotation.SuppressLint;
+import android.util.Log;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.Blinker;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.IMU;
+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.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+
+import java.util.ArrayList;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.CopyOnWriteArrayList;
+import java.util.concurrent.TimeUnit;
+
+public class Robot {
+    private static final String TAG = "Phoenix | Robot";
+    public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
+    public final PositionalServoController leftRiserServoController, rightRiserServoController;
+    public final Servo cameraServo;
+    public final CRServo collectorLeftServo, collectorRightServo;
+//    public final ServoImplEx gripper;
+    public final IMU imu;
+    public LynxModule expansionHub;
+
+    public final double imuAngleOffset, initialFacing;
+    public boolean armManuallyControlled = false;
+    public boolean automaticAntiTipActive = false;
+    public boolean hardwareFault = false;
+    public String hardwareFaultMessage = "";
+
+    private Status status = Status.OKAY, lastStatus = Status.OKAY;
+    private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
+    private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
+    private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
+    private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
+    private ArmPosition armTargetPosition;
+
+    public enum ArmPosition {
+        COLLECT,
+        GROUND,
+        LOW,
+        MEDIUM,
+        HIGH
+
+    }
+
+    public enum Status {
+        OKAY,
+        MONITORING,
+        WARNING,
+        DANGER
+    }
+
+    private final CyberarmEngine engine;
+    private TimeCraftersConfiguration configuration;
+    private final double radius, diameter;
+
+    private final double wheelRadius, wheelGearRatio, armGearRatio;
+    private final int wheelTicksPerRevolution, armTicksPerRevolution;
+
+    private final VuforiaLocalizer vuforia;
+    private final TFObjectDetector tfod;
+
+    private boolean LEDStatusToggle = false;
+    private double lastLEDStatusAnimationTime = 0;
+
+    public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) {
+        this.engine = engine;
+        this.configuration = configuration;
+
+        radius = tuningConfig("field_localizer_robot_radius").value();
+        diameter = radius * 2;
+
+        imuAngleOffset = hardwareConfig("imu_angle_offset").value();
+
+        wheelRadius = tuningConfig("wheel_radius").value();
+        wheelGearRatio = tuningConfig("wheel_gear_ratio").value();
+        wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
+
+        armGearRatio = tuningConfig("arm_gear_ratio").value();
+        armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
+
+        // FIXME: Rename motors in configuration
+        // Define hardware
+        backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Left");     // MOTOR PORT: 2 - CONTROL HUB
+        frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Right"); // MOTOR PORT: 1 - CONTROL HUB
+
+        frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Left");   // MOTOR PORT: 3 - CONTROL HUB
+        backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Right");   // MOTOR PORT: 0 - CONTROL HUB
+
+        // FIXME: Rename lift_drive to arm in hardware config
+        arm = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor");          // MOTOR PORT: 2 - EXPANSION HUB
+
+//        gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper");       // SERVO PORT: ?
+
+        imu = engine.hardwareMap.get(IMU.class, "imu");
+
+
+        // Configure hardware
+        //   MOTORS
+        //      DIRECTION
+        frontLeftDrive.setDirection(hardwareConfig("front_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+        frontRightDrive.setDirection(hardwareConfig("front_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        backLeftDrive.setDirection(hardwareConfig("back_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+        backRightDrive.setDirection(hardwareConfig("back_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        arm.setDirection(hardwareConfig("arm_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        //      RUNMODE
+        frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        arm.setTargetPosition(0);
+        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        armTargetPosition = ArmPosition.COLLECT;
+
+        //      ZERO POWER BEHAVIOR
+        frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        //      MOTOR POWER
+        arm.setPower(tuningConfig("arm_automatic_power").value());
+
+        //   SERVOS
+        Servo leftRiser = engine.hardwareMap.servo.get("LowRiserLeft");
+        leftRiser.setDirection(Servo.Direction.FORWARD);
+        Servo rightRiser = engine.hardwareMap.servo.get("LowRiserRight");
+        rightRiser.setDirection(Servo.Direction.REVERSE);
+
+        //      NOTES: 428.57142858° per second = no load speed of 60° per 0.14s at 6V, see: https://docs.revrobotics.com/duo-build/actuators/servos/smart-robot-servo#mechanical-specifications
+        leftRiserServoController = new PositionalServoController(
+                leftRiser,
+                5.0,
+                428.57142858,
+                270.0
+        ); // SERVER PORT: ? - ? HUB
+
+        rightRiserServoController = new PositionalServoController(
+                rightRiser,
+                5.0,
+                428.57142858,
+                270.0
+        ); // SERVER PORT: ? - ? HUB
+
+        cameraServo = engine.hardwareMap.servo.get("Camera Servo"); // SERVER PORT: ? - ? HUB
+
+        collectorLeftServo = engine.hardwareMap.crservo.get("Collector Left");   // SERVER PORT: ? - ? HUB
+        collectorRightServo = engine.hardwareMap.crservo.get("Collector Right"); // SERVER PORT: ? - ? HUB
+
+        //      SERVO DIRECTIONS
+        cameraServo.setDirection(Servo.Direction.REVERSE);
+        collectorLeftServo.setDirection(DcMotorSimple.Direction.REVERSE);
+        collectorRightServo.setDirection(DcMotorSimple.Direction.FORWARD);
+
+        //  SENSORS
+        //      IMU
+        IMU.Parameters parameters = new IMU.Parameters(
+            new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                RevHubOrientationOnRobot.UsbFacingDirection.UP
+            )
+        );
+
+        imu.initialize(parameters);
+
+        // Preserve our "initial" facing, since we transform it from zero.
+        initialFacing = facing();
+
+        // BulkRead from Hubs
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+
+            if (!hub.isParent() && expansionHub == null) {
+                expansionHub = hub;
+            }
+        }
+
+        // Set LED pattern
+        if (expansionHub != null) {
+            expansionHub.setPattern(ledPatternOkay());
+        }
+
+        // Webcam
+        vuforia = initVuforia();
+        tfod = initTfod();
+
+        // Drive Encoder Error Setup
+        engine.blackboardSet("left_drive_error", 0);
+        engine.blackboardSet("right_drive_error", 0);
+    }
+
+    private VuforiaLocalizer initVuforia() {
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+
+        parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value();
+        parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
+
+         return ClassFactory.getInstance().createVuforia(parameters);
+    }
+
+    private TFObjectDetector initTfod() {
+        int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        tfodParameters.minResultConfidence = 0.75f;
+        tfodParameters.isModelTensorFlow2 = true;
+        tfodParameters.inputSize = 300;
+        TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
+
+        tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel");
+
+        return tfod;
+    }
+
+    public void reloadConfig() {
+        String name = configuration.getConfig().getName();
+        configuration = new TimeCraftersConfiguration(name);
+    }
+
+    public void standardTelemetry() {
+        engine.telemetry.addLine();
+
+        // STATUS
+        engine.telemetry.addLine("DATA");
+        engine.telemetry.addData("      Robot Status", status);
+        engine.telemetry.addData("      Hardware Fault", hardwareFault);
+        engine.telemetry.addData("      Hardware Fault Message", hardwareFaultMessage);
+        engine.telemetry.addLine();
+
+        // Motor Powers
+        engine.telemetry.addLine("Motor Powers");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getPower());
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getPower());
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getPower());
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getPower());
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getPower());
+
+        engine.telemetry.addLine();
+
+        // Motor Positions
+        engine.telemetry.addLine("Motor Positions");
+        engine.telemetry.addData("      Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getCurrentPosition()));
+        engine.telemetry.addData("      Front Right Drive", "%d (%8.2f in)", frontRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getCurrentPosition()));
+
+        engine.telemetry.addData("      Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition()));
+        engine.telemetry.addData("      Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition()));
+
+        engine.telemetry.addLine();
+
+        // Motor Target Positions
+        engine.telemetry.addLine("Motor Target Positions");
+        engine.telemetry.addData("      Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getTargetPosition()));
+        engine.telemetry.addData("      Front Right Drive", "%d (%8.2f in)", frontRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getTargetPosition()));
+
+        engine.telemetry.addData("      Back Left Drive", "%d (%8.2f in)", backLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getTargetPosition()));
+        engine.telemetry.addData("      Back Right Drive", "%d (%8.2f in)", backRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getTargetPosition()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
+
+        engine.telemetry.addLine();
+
+        // Motor Velocity
+        engine.telemetry.addLine("Motor Velocity");
+        engine.telemetry.addData("      Front Left Drive", "%8.2f (%8.2f in)", frontLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontLeftDrive.getVelocity()));
+        engine.telemetry.addData("      Front Right Drive", "%8.2f (%8.2f in)", frontRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontRightDrive.getVelocity()));
+
+        engine.telemetry.addData("      Back Left Drive", "%8.2f (%8.2f in)", backLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backLeftDrive.getVelocity()));
+        engine.telemetry.addData("      Back Right Drive", "%8.2f (%8.2f in)", backRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backRightDrive.getVelocity()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%8.2f (%8.2f degrees)", arm.getVelocity(), ticksToAngle((int)arm.getVelocity()));
+
+        engine.telemetry.addLine();
+
+        // Motor Currents
+        engine.telemetry.addLine("Motor Currents (AMPS)");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getCurrent(CurrentUnit.AMPS));
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS));
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addLine();
+
+        // Motor Directions
+        engine.telemetry.addLine("Motor Directions");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getDirection());
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getDirection());
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getDirection());
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getDirection());
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getDirection());
+
+        engine.telemetry.addLine();
+
+        // Servo Positions
+        engine.telemetry.addLine("Servo Positions");
+        engine.telemetry.addData("      Left Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", leftRiserServoController.getEstimatedPosition(), leftRiserServoController.getError(), leftRiserServoController.getTargetPosition(), leftRiserServoController.getServo().getPosition());
+        engine.telemetry.addData("      Right Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", rightRiserServoController.getEstimatedPosition(), rightRiserServoController.getError(), rightRiserServoController.getTargetPosition(), rightRiserServoController.getServo().getPosition());
+        engine.telemetry.addData("      Camera (Blind)", cameraServo.getPosition());
+
+        engine.telemetry.addLine();
+
+        // Continuous Servo Powers
+        engine.telemetry.addLine("Servo Powers");
+        engine.telemetry.addData("      Collector Left", collectorLeftServo.getPower());
+        engine.telemetry.addData("      Collector Right", collectorRightServo.getPower());
+
+        engine.telemetry.addLine();
+
+        // Servo Directions
+        engine.telemetry.addLine("Servo Directions");
+        engine.telemetry.addData("      Left Riser", leftRiserServoController.getServo().getDirection());
+        engine.telemetry.addData("      Right Riser", rightRiserServoController.getServo().getDirection());
+        engine.telemetry.addData("      Camera", cameraServo.getDirection());
+        engine.telemetry.addData("      Collector Left", collectorLeftServo.getDirection());
+        engine.telemetry.addData("      Collector Right", collectorRightServo.getDirection());
+
+        engine.telemetry.addLine();
+
+        // Sensors / IMU
+        engine.telemetry.addLine("IMU");
+        engine.telemetry.addData("      Facing (Degrees)", facing());
+        engine.telemetry.addData("      Heading (Radians)", heading());
+        engine.telemetry.addData("      Turn Rate", turnRate());
+        engine.telemetry.addData("      Angle Offset (Degrees)", imuAngleOffset);
+
+        engine.telemetry.addLine();
+    }
+
+    private ArrayList<Blinker.Step> ledPatternStandby() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0x008000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x005000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x002000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x001000, 250, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternOkay() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternMonitoring() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternWarning() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternDanger() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    public void reportStatus(Status status) {
+        reportedStatuses.add(status);
+    }
+
+    public void update() {
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.clearBulkCache();
+        }
+
+        double voltage = getVoltage();
+        if (voltage < 9.75) {
+            reportStatus(Status.DANGER);
+            hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)";
+
+            hardwareFault = true;
+        }
+
+        status = Status.OKAY;
+        for (Status s : reportedStatuses) {
+            if (s.ordinal() > status.ordinal()) {
+                status = s;
+            }
+        }
+
+        reportedStatuses.clear();
+
+        if (status != lastStatus) {
+            lastStatus = status;
+
+            if (expansionHub != null) {
+                if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); }
+                if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); }
+                if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); }
+                if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
+            }
+        }
+
+        manageArmAndRiserServos();
+    }
+
+    public void stop() {
+        if (expansionHub != null) {
+            expansionHub.setPattern(ledPatternStandby());
+        }
+    }
+
+    public void armPosition(ArmPosition position) {
+        if (hardwareFault) {
+            return;
+        }
+
+        armTargetPosition = position;
+
+        reportStatus(Status.WARNING);
+    }
+
+    private void manageArmAndRiserServos() {
+        boolean lowerServos = true;
+
+        switch (armTargetPosition) {
+            case COLLECT:
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
+                }
+                break;
+
+//            case GROUND:
+//                if (areRiserServosInLoweredPosition()) {
+//                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
+//                }
+//                break;
+
+            case GROUND: // DISABLE GROUND JUNCTION SUPPORT FOR NOW
+            case LOW:
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
+                }
+                break;
+
+            case MEDIUM:
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
+                }
+                break;
+
+            case HIGH:
+                double angleHigh = tuningConfig("arm_position_angle_high").value();
+                arm.setTargetPosition(angleToTicks(angleHigh));
+
+                if (arm.getCurrentPosition() >= angleToTicks(angleHigh - 10.0)) {
+                    lowerServos = false;
+                }
+                break;
+
+            default:
+                throw new RuntimeException("Unexpected arm position!");
+        }
+
+        if (lowerServos) {
+            leftRiserServoController.setTargetPosition(0.45);
+            rightRiserServoController.setTargetPosition(0.45);
+        } else {
+            leftRiserServoController.setTargetPosition(0.8);
+            rightRiserServoController.setTargetPosition(0.8);
+        }
+
+        leftRiserServoController.update();
+        rightRiserServoController.update();
+    }
+
+    private boolean areRiserServosInLoweredPosition() {
+        return leftRiserServoController.getEstimatedPosition() < 0.5 && rightRiserServoController.getEstimatedPosition() < 0.5;
+    }
+
+    // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
+    public double angleDiff(double from, double to) {
+        double value = (to - from + 180);
+
+        double fmod = (value - 0.0) % (360.0 - 0.0);
+
+        return (fmod < 0 ? fmod + 360.0 : fmod +  0.0) - 180;
+    }
+
+    public double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+
+    public Status getStatus() { return status; }
+
+    public double getRadius() { return radius; }
+
+    public double getDiameter() { return diameter; }
+
+    public double getVoltage() {
+        return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
+    }
+
+    public TFObjectDetector getTfod() { return tfod; }
+
+    public VuforiaLocalizer getVuforia() { return vuforia; }
+
+    public TimeCraftersConfiguration getConfiguration() { return configuration; }
+
+    // For: Drive Wheels
+    public int unitToTicks(DistanceUnit unit, double distance) {
+        double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution));
+
+        double inches = unit.toInches(unit.fromUnit(unit, distance));
+
+        double ticks = fI * inches;
+
+        return (int)ticks;
+    }
+
+    // For: Drive Wheels
+    public double ticksToUnit(DistanceUnit unit, int ticks) {
+        // Convert to inches, then to unit.
+        double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution);
+
+        return unit.fromUnit(DistanceUnit.INCH, inches);
+    }
+
+    // For: Arm
+    public int angleToTicks(double angle) {
+        double d = (armGearRatio * armTicksPerRevolution) / 360.0;
+
+        // Casting to float so that the int version of Math.round is used.
+        return Math.round((float)d * (float)angle);
+    }
+
+    // For: Arm
+    public double ticksToAngle(int ticks) {
+        double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution);
+
+        return oneDegree * ticks;
+    }
+
+    public Variable hardwareConfig(String variableName) {
+        Action hardwareConfiguration = configuration.action("Robot", "Hardware");
+
+        for (Variable v : hardwareConfiguration.getVariables()) {
+            if (variableName.trim().equals(v.name)) {
+                return v;
+            }
+        }
+
+        throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Hardware");
+    }
+
+    public Variable tuningConfig(String variableName) {
+        Action action = configuration.action("Robot", "Tuning");
+
+        for (Variable v : action.getVariables()) {
+            if (variableName.trim().equals(v.name)) {
+                return v;
+            }
+        }
+
+        throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning");
+    }
+
+    @SuppressLint("NewApi")
+    public void controlMotorPIDF(DcMotorEx motor, String motorName, double targetVelocity, double feedForward) {
+        Action action = configuration.action("Robot", "Tuning_PIDF_" + motorName);
+        double proportional = 0, integral = 0, derivative = 0;
+
+        for (Variable v : action.getVariables()) {
+            switch (v.name.trim()) {
+                case "proportional":
+                    proportional = v.value();
+                    break;
+                case "integral":
+                    integral = v.value();
+                    break;
+                case "derivative":
+                    derivative = v.value();
+                    break;
+            }
+        }
+
+        double interval = (engine.getRuntime() - motorVelocityLastTiming.getOrDefault(motorName, 0.0));
+
+        double distance = motor.getTargetPosition() - motor.getCurrentPosition();
+
+        if (Math.abs(distance) < Math.abs(targetVelocity)) {
+            if ((targetVelocity < 0 && distance > 0) || (targetVelocity > 0 && distance < 0)) {
+                targetVelocity = -distance;
+            } else {
+                targetVelocity = distance;
+            }
+        }
+
+        double error = targetVelocity - motor.getVelocity();
+        double deltaError = error - motorVelocityError.getOrDefault(motorName, 0.0);
+
+        double kIntegral = error * interval;
+        double kDerivative = deltaError / interval;
+
+        double kp = proportional * error;
+        double ki = integral * kIntegral;
+        double kd = derivative * kDerivative;
+
+        motorVelocityError.put(motorName, error);
+        motorVelocityLastTiming.put(motorName, engine.getRuntime());
+
+        double newTargetVelocity = kp + ki + kd + targetVelocity;
+
+        motor.setVelocity(newTargetVelocity);
+
+        Log.d(TAG, "Interval: " + interval + "ms, Error: " + error + " ticks, deltaError: " + deltaError + " ticks, distance: " +
+                distance + " ticks, kIntegral: " + kIntegral + ", kDerivative: " + kDerivative + ", proportional: " + proportional +
+                ", integral: " + integral + ", derivative: " + derivative + ", kp: " + kp + ", ki: " + ki + ", kd: " + kd +
+                ", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks.");
+    }
+
+    @SuppressLint("NewApi")
+    public void controlArmMotor(double targetVelocity) {
+//        double time = System.currentTimeMillis();
+//        double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
+//        double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
+//        double deltaTime = (time - lastTiming) * 0.001;
+//
+//        double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
+//        double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
+//
+//        double error = adjustedTargetVelocity - arm.getVelocity();
+//        double kp = 0.9;
+//
+//        newTargetVelocity += error * kp * deltaTime;
+//
+//        motorTargetVelocity.put("Arm", newTargetVelocity);
+//        motorVelocityLastTiming.put("Arm", time);
+
+//        arm.setVelocity(newTargetVelocity);
+
+        arm.setPower(tuningConfig("arm_automatic_power").value());
+    }
+
+    public double initialFacing() {
+        return initialFacing;
+    }
+
+    public double facing() {
+        double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+        return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
+    }
+
+    public double heading() {
+        return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
+//        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    public double turnRate() {
+        return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
+    }
+
+    public boolean isBetween(double value, double min, double max) {
+        return value >= min && value <= max;
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java

@@ -0,0 +1,47 @@
+package org.timecrafters.minibots.cyberarm.phoenix.engines;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.minibots.cyberarm.phoenix.Robot;
+
+@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Mentor Phoenix | TeleOp", group = "Mentor Phoenix")
+public class TeleOp extends CyberarmEngine {
+    private Robot robot;
+    private TimeCraftersConfiguration configuration;
+    final private String configurationName = "MentorPhoenix", actionsGroupName = "TeleOpStates";
+
+    @Override
+    public void setup() {
+        configuration = new TimeCraftersConfiguration(configurationName);
+
+        robot = new Robot(
+                this,
+                configuration
+        );
+
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.getConfiguration(),
+                "org.timecrafters.minibots.cyberarm.phoenix.states.teleop",
+                robot,
+                Robot.class,
+                actionsGroupName);
+    }
+
+    @Override
+    public void loop() {
+        robot.update();
+
+        super.loop();
+
+        robot.standardTelemetry();
+    }
+
+    @Override
+    public void stop() {
+        robot.stop();
+
+        super.stop();
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java

@@ -0,0 +1,122 @@
+package org.timecrafters.minibots.cyberarm.phoenix.states.teleop;
+
+import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+import org.timecrafters.minibots.cyberarm.phoenix.Robot;
+
+public class ArmController extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+    private final Gamepad controller;
+    private PIDController pidController;
+    private double p = 0, i = 0, d = 0, f = 0;
+    private final double ticksInDegree = 700 / 180;
+
+    public ArmController(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.controller = engine.gamepad1;
+
+        pidController = new PIDController(p, i, d);
+    }
+
+    @Override
+    public void exec() {
+        Action action = robot.getConfiguration().action("Robot", "Tuning_PIDF_Arm");
+
+        for (Variable v : action.getVariables()) {
+            switch (v.name.trim()) {
+                case "proportional":
+                    p = v.value();
+                    break;
+                case "integral":
+                    i = v.value();
+                    break;
+                case "derivative":
+                    d = v.value();
+                    break;
+                case "feed": // feedback
+                    f = v.value();
+                    break;
+            }
+        }
+
+        pidController.setPID(p, i, d);
+        int armPos = robot.arm.getCurrentPosition();
+        double pid = pidController.calculate(armPos, robot.arm.getTargetPosition());
+        double ff = Math.cos(Math.toRadians(robot.arm.getTargetPosition() / ticksInDegree)) * f;
+
+        double power = pid + ff;
+
+        robot.arm.setPower(power);
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+
+        switch (button) {
+            case "guide":
+                robot.reloadConfig();
+                break;
+
+            // Arm control
+            case "a":
+                robot.armPosition(Robot.ArmPosition.COLLECT);
+                break;
+            case "x":
+                robot.armPosition(Robot.ArmPosition.LOW); // DISABLED GROUND JUNCTION
+                break;
+            case "b":
+                robot.armPosition(Robot.ArmPosition.MEDIUM);
+                break;
+            case "y":
+                robot.armPosition(Robot.ArmPosition.HIGH);
+                break;
+            // Manual stepping arm
+            case "left_bumper":
+                robot.arm.setTargetPosition(
+                        robot.arm.getCurrentPosition() - robot.angleToTicks(5.0)
+                );
+                break;
+            case "right_bumper":
+                robot.arm.setTargetPosition(
+                        robot.arm.getCurrentPosition() + robot.angleToTicks(5.0)
+                );
+                break;
+
+            // Collector control
+            case "dpad_up":
+                robot.collectorLeftServo.setPower(-1);
+                robot.collectorRightServo.setPower(-1);
+                break;
+            case "dpad_down":
+                robot.collectorLeftServo.setPower(1);
+                robot.collectorRightServo.setPower(1);
+        }
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+
+        switch (button) {
+            // Collector control
+            case "dpad_up":
+            case "dpad_down":
+                robot.collectorLeftServo.setPower(0);
+                robot.collectorRightServo.setPower(0);
+                break;
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java

@@ -0,0 +1,118 @@
+package org.timecrafters.minibots.cyberarm.phoenix.states.teleop;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.minibots.cyberarm.phoenix.Robot;
+
+public class DriveController extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+
+    private final Gamepad controller;
+    private boolean robotSlowMode;
+
+    public DriveController(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.controller = engine.gamepad1;
+
+        this.robotSlowMode = true;
+    }
+
+    @Override
+    public void exec() {
+        move();
+    }
+
+    @Override
+    public void stop() {
+        stopDrive();
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+
+        // DEBUG: Toggle hardware fault
+        if (button.equals("guide")) {
+            robot.hardwareFault = !robot.hardwareFault;
+
+            if (robot.hardwareFault) {
+                robot.hardwareFaultMessage = "Manually triggered.";
+            } else {
+                robot.hardwareFaultMessage = "";
+            }
+        }
+
+        if (button.equals("right_stick_button")) {
+            robotSlowMode = !robotSlowMode;
+        }
+
+        if (button.equals("left_stick_button") && robot.hardwareFault) {
+            robot.imu.resetYaw();
+        }
+    }
+
+    // REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
+    private void move() {
+        if (robot.automaticAntiTipActive || robot.hardwareFault) {
+            return;
+        }
+
+        double x = controller.left_stick_x;
+        double y = -controller.left_stick_y;
+
+        // Improve control?
+        if (y < 0) {
+            y = -Math.sqrt(-y);
+        } else {
+            y = Math.sqrt(y);
+        }
+
+        if (x < 0) {
+            x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing;
+        } else {
+            x = Math.sqrt(x) * 1.1; // Counteract imperfect strafing;
+        }
+
+        double rx = -controller.right_stick_x;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+
+
+        double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
+
+        double heading = robot.heading();
+        double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+        double rotY = x * Math.sin(heading) + y * Math.cos(heading);
+
+        frontLeftPower = (rotY + rotX + rx) / denominator;
+        backLeftPower = (rotY - rotX - rx) / denominator;
+        frontRightPower = (rotY - rotX + rx) / denominator;
+        backRightPower = (rotY + rotX - rx) / denominator;
+
+        double maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value());
+        double slowVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_slow_velocity_in_inches").value());
+        double velocity = robotSlowMode ? slowVelocity : maxVelocity;
+
+        // Power is treated as a ratio here
+        robot.frontLeftDrive.setVelocity(frontLeftPower * velocity);
+        robot.frontRightDrive.setVelocity(frontRightPower * velocity);
+
+        robot.backLeftDrive.setVelocity(backLeftPower * velocity);
+        robot.backRightDrive.setVelocity(backRightPower * velocity);
+    }
+
+    private void stopDrive() {
+        robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
+        robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
+
+        robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
+        robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/engines/Mini2023Engine.java

@@ -0,0 +1,22 @@
+package org.timecrafters.minibots.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.minibots.states.Mini2023Bot;
+import org.timecrafters.minibots.states.Mini2023State;
+
+@TeleOp (name= "2023Mini")
+
+public class Mini2023Engine extends CyberarmEngine {
+
+    Mini2023Bot robot;
+
+    @Override
+    public void setup() {
+
+        robot = new Mini2023Bot(this);
+        addState(new Mini2023State(robot));
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java

@@ -0,0 +1,49 @@
+package org.timecrafters.minibots.states;
+
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.minibots.engines.Mini2023Engine;
+
+public class Mini2023Bot {
+
+    private final Mini2023Engine engine;
+    public TimeCraftersConfiguration configuration;
+
+        public DcMotor Opportunity, Spirit; //Don't ask. Just don't.
+        public CRServo servoA, servoB, servoC; //Just be thankful the servos have 'normal' names.
+        public IMU imu;
+        public ModernRoboticsI2cRangeSensor hazcam;
+
+    public Mini2023Bot(Mini2023Engine engine) {
+        this.engine = engine;
+        setupRobot();
+    }
+
+    private void setupRobot() {
+
+        Spirit = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
+        Opportunity = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
+
+        servoA = engine.hardwareMap.get(CRServo.class, "Servo 1");
+        servoB = engine.hardwareMap.get(CRServo.class, "Servo 2");
+        servoC = engine.hardwareMap.get(CRServo.class, "Servo 3");
+
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                RevHubOrientationOnRobot.UsbFacingDirection.UP
+        ));
+
+        this.imu = engine.hardwareMap.get(IMU.class, "imu");
+        imu.initialize(parameters);
+
+
+
+//        configuration = new TimeCraftersConfiguration("2023 Mini");
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java

@@ -0,0 +1,77 @@
+package org.timecrafters.minibots.states;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Mini2023State extends CyberarmState {
+    private final Mini2023Bot robot;
+
+    public double lThrust, rThrust, orbitSpeed;
+
+    public double deltaServo;
+
+    public Mini2023State(Mini2023Bot robot) {this.robot = robot;}
+
+    public double getDeltaServo() {
+
+        return deltaServo;
+    }
+
+    @Override public void telemetry() {
+        engine.telemetry.addData("Right Drive Power", robot.Opportunity.getPower());
+        engine.telemetry.addData("Left Drive Power", robot.Spirit.getPower());
+}
+
+@Override
+public void init() {
+    lThrust = 0;
+    rThrust = 0;
+    orbitSpeed = 0;
+    robot.servoA.setPower(orbitSpeed);
+    robot.servoB.setPower(orbitSpeed);
+    robot.servoC.setPower(orbitSpeed);
+    robot.Spirit.setPower(lThrust);
+    robot.Opportunity.setPower(rThrust);
+}
+
+@Override
+public void exec() {
+
+    if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
+        lThrust = engine.gamepad1.left_stick_y * 0.5;
+        robot.Spirit.setPower(lThrust);
+    } else {
+        lThrust = 0;
+        robot.Spirit.setPower(lThrust);
+    }
+
+    if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
+        rThrust = engine.gamepad1.right_stick_y * 0.5;
+        robot.Opportunity.setPower(rThrust);
+    } else {
+        rThrust = 0;
+        robot.Opportunity.setPower(rThrust);
+    }
+
+    if (engine.gamepad1.right_trigger > 0.1) {
+        orbitSpeed = engine.gamepad1.right_trigger * 0.5;
+        robot.servoA.setPower(orbitSpeed);
+        robot.servoB.setPower(orbitSpeed);
+        robot.servoC.setPower(orbitSpeed);
+    } else if (engine.gamepad1.left_trigger > 0.1) {
+        orbitSpeed = engine.gamepad1.left_trigger * 0.5;
+        robot.servoA.setPower(orbitSpeed);
+        robot.servoB.setPower(orbitSpeed);
+        robot.servoC.setPower(orbitSpeed);
+    } else {
+        orbitSpeed = 0;
+        robot.servoA.setPower(orbitSpeed);
+        robot.servoB.setPower(orbitSpeed);
+        robot.servoC.setPower(orbitSpeed);
+    }
+
+
+
+}
+}

build.common.gradle

@@ -51,7 +51,7 @@ android {
     defaultConfig {
         signingConfig signingConfigs.debug
         applicationId 'com.qualcomm.ftcrobotcontroller'
-        minSdkVersion 23
+        minSdkVersion 24
         //noinspection ExpiredTargetSdkVersion
         targetSdkVersion 28
 

build.dependencies.gradle

@@ -22,5 +22,6 @@ dependencies {
     implementation 'org.apache.commons:commons-math3:3.6.1'
     implementation 'com.acmerobotics.roadrunner:core:0.5.6'
     implementation 'com.acmerobotics.dashboard:dashboard:0.4.8'
+    implementation 'org.ftclib.ftclib:core:2.1.1'
 }