Autonomous work for driving straight, And rotation

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

@@ -14,7 +14,7 @@ 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.Autonomous.TeleOp.states.PhoenixBot1;
 
 @Autonomous (name = "Left Side")
 @Disabled

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

@@ -15,7 +15,7 @@ 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.Autonomous.TeleOp.states.PhoenixBot1;
 
 @Autonomous (name = "left 2 cone auto")
 

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

@@ -13,7 +13,7 @@ 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.Autonomous.TeleOp.states.PhoenixBot1;
 
 @Autonomous (name = "Right ")
 @Disabled

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

@@ -3,18 +3,9 @@ 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.TeleOp.states.PhoenixBot1;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
 
 @Autonomous(name = "Right Side")
 public class RightStateAutoEngine extends CyberarmEngine {
@@ -23,22 +14,32 @@ public class RightStateAutoEngine extends CyberarmEngine {
     @Override
     public void setup() {
         robot = new PhoenixBot1(this);
+        robot.imu.resetYaw();
 
-        // driving towards Low
+//        setupFromConfig(
+//                robot.configuration,
+//                "org.timecrafters.Autonomous.States",
+//                robot,
+//                PhoenixBot1.class,
+//                "Right State Auto");
+
+//        // driving forward to low junction
         addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
-        // rotate towards low
+//        // rotate left towards low junction
-        addState(new RotationState(robot, "Right State Auto", "02-1"));
+//        addState(new RotationState(robot, "Right State Auto", "02-1"));
-        // drive forwards or backwards to adjust to pole
+//        // driving forward/ backwards to adjust
-        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
-        // counteract distance driven
+//        // drive forwards or backwards to adjust to pole
-        addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
-        // rotate towards opposing alliance
+//        // counteract distance driven
-        addState(new RotationState(robot, "Right State Auto", "04-1"));
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
-        // drive to stack column
+//        // rotate towards opposing alliance
-        addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0"));
+//        addState(new RotationState(robot, "Right State Auto", "04-1"));
-        // rotate at stack
+//        // drive to stack column
-        addState(new RotationState(robot, "Right State Auto", "05-1"));
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0"));
-        // drive at stack
+//        // 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/Engines/RightTwoConeAutonomousEngine.java

@@ -15,7 +15,7 @@ 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.Autonomous.TeleOp.states.PhoenixBot1;
 
 @Autonomous (name = "Right 2 cone auto")
 

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

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

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

@@ -4,7 +4,7 @@ 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.Autonomous.TeleOp.states.PhoenixBot1;
 
 public class CollectorDistanceState extends CyberarmState {
 

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

@@ -2,7 +2,7 @@ package org.timecrafters.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.Autonomous.TeleOp.states.PhoenixBot1;
 
 public class CollectorState extends CyberarmState {
 

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

@@ -2,7 +2,7 @@ package org.timecrafters.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.Autonomous.TeleOp.states.PhoenixBot1;
 
 import java.util.List;
 

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

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

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

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

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

@@ -1,12 +1,10 @@
 package org.timecrafters.Autonomous.States;
 
-import android.util.Log;
-
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
 
 public class DriverStateWithOdometer extends CyberarmState {
     private final boolean stateDisabled;
@@ -16,10 +14,20 @@ public class DriverStateWithOdometer extends CyberarmState {
     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 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 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();
@@ -28,10 +36,10 @@ public class DriverStateWithOdometer extends CyberarmState {
         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;
 
@@ -44,143 +52,257 @@ public class DriverStateWithOdometer extends CyberarmState {
         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)) * distanceMultiplier);
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
-        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
+        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
-        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
+        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
-        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
+        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
 
 
+        if (targetDrivePower > 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;
+
+        }
+
+        driveStage = 0;
 
     }
 
     @Override
     public void exec() {
 
+
         if (stateDisabled) {
             setHasFinished(true);
             return;
         }
 
-        double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
+        double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
-        double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
+        double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
 
-        if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
+        // Driving Forward
-        if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
+        if (targetDrivePower > 0 && driveStage == 0) {
 
-
-        if (Math.abs(CurrentPosition) <= RampUpDistance){
             // ramping up
-//            double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
+            if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
-            if (targetDrivePower > 0) {
+                    (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
-                drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
+
-            } else {
+                drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
-                drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25;
+                        (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
-            }
+
-        }
-        else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
-            // ramping down
-            if (targetDrivePower > 0){
-                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
-            } else {
-                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
             }
 
-        } else {
+            // Driving Normal
-            // middle ground
+            else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
+                    (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
+
                 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 > 0) {
-            drivePower = drivePower * -1;
-        }
-
-        if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){
-            if (targetAchieved) {
-                drivePower = drivePower * 0.25;
-
-                if (Math.abs(drivePower) < 0.25){
-                    if (drivePower < 0) {
-                        drivePower = -0.25;
-                    } else {
-                        drivePower = 0.25;
-                    }
-                }
-            }
-            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.25;
-
-            if (Math.abs(drivePower) < 0.25){
-                if (drivePower < 0) {
-                    drivePower = -0.25;
-                } else {
-                    drivePower = 0.25;
-                }
-            }
-
-            robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
-
 
             }
 
-        else {
+            // 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;
 
-            if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){
+            } else if (driveStage == 0){
-
+                driveStage = 1;
-                if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
-                    drivePower = 0;
-                } else {
-                    drivePower = 0.25;
-                    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.25;
-                    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);
+                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);
+        }
     }
 
     @Override
     public void telemetry () {
+        engine.telemetry.addData("Stage", driveStage);
+        engine.telemetry.addData("maximumTolerance", maximumTolerance);
+        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());
@@ -189,15 +311,13 @@ public class DriverStateWithOdometer extends CyberarmState {
         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("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
-        engine.telemetry.addData("imu yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
-        engine.telemetry.addData("imu pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
+        engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
-        engine.telemetry.addData("imu roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
+        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);
 
@@ -205,9 +325,6 @@ public class DriverStateWithOdometer extends CyberarmState {
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
 
-        Log.i("TELEMETRY", "imu yaw:: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
-        Log.i("TELEMETRY", "imu pitch:: " + robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
-        Log.i("TELEMETRY", "imu roll:: " + robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
 
     }
 }

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

@@ -5,7 +5,7 @@ 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.Autonomous.TeleOp.states.PhoenixBot1;
 
 public class JunctionAllignmentAngleState extends CyberarmState {
     private final boolean stateDisabled;

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

@@ -1,10 +1,8 @@
 package org.timecrafters.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.Autonomous.TeleOp.states.PhoenixBot1;
 
 public class JunctionAllignmentDistanceState extends CyberarmState {
     private final boolean stateDisabled;

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

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

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

@@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
 
 public class RotationState extends CyberarmState {
     private final boolean stateDisabled;
@@ -11,7 +11,7 @@ public class RotationState extends CyberarmState {
         this.robot = robot;
         this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
         this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
-        drivePowerVariable = drivePower;
+        this.ClockWiseRotation = robot.configuration.variable(groupName, actionName, "ClockWiseRotation").value();
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
 
 
@@ -19,19 +19,23 @@ public class RotationState extends CyberarmState {
 
     private double drivePower;
     private float targetRotation;
-    float RobotRotation;
+    float CurrentRotation;
-    private double RotationTarget;
-    private double RotationDirectionMinimum;
     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
@@ -39,49 +43,75 @@ public class RotationState extends CyberarmState {
         if (stateDisabled) {
             setHasFinished(true);
             return;
-            } // end of if
+        } //
 
-        RobotRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
-        if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
+        CurrentRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
-            drivePowerVariable = 0.4 * drivePower;
+
-            if (Math.abs(drivePowerVariable) < 0.4) {
+        if (RotationStage == 0) {
-                if (drivePowerVariable < 0){
+
-                    drivePowerVariable = -0.4;
+            drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER;
-                } else {
+
-                    drivePowerVariable = 0.4;
+            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();
             }
             }
-            debugStatus = "Rotate Slow";
-        } // end of if
-        else {
-            drivePowerVariable = drivePower * 0.75;
-            debugStatus = "Rotate Fast";
-        }
 
-        if (RobotRotation >= targetRotation + 1){
+        if (RotationStage == 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;
+            if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION ){
-            PhoenixBot1.RightCompensatorGlobal = (RightCompensator + robot.OdometerEncoderRight.getCurrentPosition()) - RightCompensator;
+                RotationStage = 2;
-            setHasFinished(true);
+            }
         }
 
+        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  );
+
             }
+            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);
+
+            } else {
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
+
+                RotationStage ++;
+//                setHasFinished(true);
+            }
+        }
+
+
+        }
+
+
 
     @Override
     public void telemetry() {
@@ -89,13 +119,14 @@ public class RotationState extends CyberarmState {
 
         engine.telemetry.addLine();
 
-        engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
+        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/Autonomous/States/ServoCameraRotate.java

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

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

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

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

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

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

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

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

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

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

@@ -1,12 +1,11 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Autonomous.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.PhoenixTeleOPState;
+import org.timecrafters.Autonomous.TeleOp.states.TeleOPTankDriver;
-import org.timecrafters.TeleOp.states.TeleOPArmDriver;
+import org.timecrafters.Autonomous.TeleOp.states.TeleOPArmDriver;
-import org.timecrafters.TeleOp.states.TeleOPTankDriver;
 
 @TeleOp (name = "APhoenixTeleOP")
 

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

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

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

@@ -1,10 +1,10 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Autonomous.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.Autonomous.TeleOp.states.LaserState;
 
 @Disabled
 @TeleOp(name = "Wheel")

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

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Autonomous.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.Autonomous.TeleOp.states.PhoenixBot1;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Autonomous.TeleOp.states.TeleOPSpeedrunState;
-import org.timecrafters.TeleOp.states.TeleOPSpeedrunState;
 
 @Disabled
 @TeleOp (name = "Speedrun Engine")

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

@@ -1,12 +1,11 @@
-package org.timecrafters.TeleOp.engine;
+package org.timecrafters.Autonomous.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.Autonomous.TeleOp.states.PhoenixBot1;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.Autonomous.TeleOp.states.SteeringDriveExperiment;
-import org.timecrafters.TeleOp.states.SteeringDriveExperiment;
 
 @Disabled
 @TeleOp(name = "Steering Drive Test")

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

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

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

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

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

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

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

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

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

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

TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java

@@ -1,4 +1,4 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Autonomous.TeleOp.states;
 
 import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
 import com.qualcomm.hardware.bosch.BNO055IMU;
@@ -25,7 +25,7 @@ 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;
@@ -35,6 +35,8 @@ public class PhoenixBot1 {
     public double STRAFE_MINIMUM_POWER;
     public double DRIVE_TOLERANCE;
     public double ROTATION_TOLERANCE;
+    public long PAUSE_ON_ROTATION;
+    public double DISTANCE_MULTIPLIER;
 
 //    private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
     private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -99,6 +101,9 @@ public class PhoenixBot1 {
             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();
         }
 
     private void initVuforia(){
@@ -244,6 +249,8 @@ public class PhoenixBot1 {
 
             CameraServo.setPosition(0.775);
 
+
+
         }
 
         public int AngleToTicks(double angle) {

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

@@ -1,9 +1,7 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Autonomous.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;

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

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

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

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

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

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

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

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

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

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

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

@@ -1,13 +1,11 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Autonomous.TeleOp.states;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 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.DistanceUnit;
 
 public class TeleOPArmDriver extends CyberarmState {
     private final PhoenixBot1 robot;

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

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

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

@@ -1,6 +1,5 @@
-package org.timecrafters.TeleOp.states;
+package org.timecrafters.Autonomous.TeleOp.states;
 
-import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.Gamepad;
 
 import org.cyberarm.engine.V2.CyberarmState;