Added steps to new Autonomous and made states that run parallel states

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

@@ -8,7 +8,14 @@ import org.timecrafters.Autonomous.States.CollectorState;
 import org.timecrafters.Autonomous.States.ConeIdentification;
 import org.timecrafters.Autonomous.States.DriverState;
 import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
+import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState;
+import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState2nd;
+import org.timecrafters.Autonomous.States.DriverStateWithOdometerUpperArmParallelState;
+import org.timecrafters.Autonomous.States.JunctionAllignmentState;
+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 = "3 cone auto right")
@@ -22,73 +29,113 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
 
         // 1 Rotate camera down at the signal
         addState(new ServoCameraRotate(robot, "RightFourCone", "01-0"));
+
         // 2 Scan custom image
         addState(new ConeIdentification(robot, "RightFourCone", "02-0"));
+
         // 3 Rotate Camera up, out of the way so it doesn't crash into stuff
         addState(new ServoCameraRotate(robot, "RightFourCone", "03-0"));
-        // 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
-        addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0"));
-        // 5 Turn Towards and look for junction with sensor
 
-        // 6 Raise lower arm while slowly driving at the junction
+        // 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
+        addState(new DriverStateWithOdometerUpperArmParallelState(robot, "RightFourCone", "04-0"));
+
+        // 5 Turn Towards and look for junction with sensor
+        addState(new RotationState(robot, "RightFourCone", "05-0"));
+
+        // 6 Raise lower arm while slowly driving at the junction (parallel state)
+        addState(new JunctionAllignmentState(robot, "RightFourCone", "06-0"));
 
         // 7 Drop top arm down on the junction to place cone
+        addState(new TopArm(robot, "RightFourCone", "07-0"));
 
         // 8 Drop cone as soon as arm is in position
+        addState(new CollectorState(robot, "RightFourCone", "08-0"));
 
         // 9 Raise arm to clear junction
+        addState(new TopArm(robot, "RightFourCone", "09-0"));
 
         // 10 Back up and bring lower arm down (parallel state)
+        addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0"));
 
         // 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
+        addState(new TopArm(robot, "RightFourCone", "11-0"));
 
-        // 12 Rotate towards stack (this might be parallel with last step)
+        // 12 Rotate towards stack
+        //filled in as parallel state. in parallel with previous state
 
         // 13 Drive at stack while collecting and check to see when we grab it
+        addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
 
-        // 14 Back up and raise arm (in parallel state)
+        // 14 Back up and raise arm
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
+        addState(new TopArm(robot, "RightFourCone", "14-1"));
 
         // 15 Drive All the way back to the medium Junction and raise upper arm (parallel state)
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
 
         // 16 Rotate and use sensor to find junction
+        addState(new RotationState(robot, "RightFourCone", "16-0"));
+        addState(new JunctionAllignmentState(robot, "RightFourCone", "16-1"));
 
         // 17 Drive Towards Junction (This is optional, idk if this is needed atm)
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
 
         // 18 Bring upper arm down
+        addState(new TopArm(robot, "RightFourCone", "18-0"));
 
         // 19 Drop cone
+        addState(new CollectorState(robot, "RightFourCone", "19-0"));
 
         // 20 Bring upper arm up
+        addState(new TopArm(robot, "RightFourCone", "20-0"));
 
         // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
 
         // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
+        addState(new TopArm(robot, "RightFourCone", "22-0"));
 
         // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
+        addState(new CollectorDistanceState(robot, "RightFourCone", "23-0"));
 
-        // 24 Drive Back and lift up all the way to position for the low junction (parallel state)
+        // 24 Drive Back and lift up all the way to position for the low junction
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "24-0"));
+        addState(new TopArm(robot, "RightFourCone", "24-1"));
 
         // 25 Drive back faster after the cone is fully off of the stack
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0"));
 
         // 26 Turn and look for the low junction with the distance sensor and align
+        addState(new RotationState(robot, "RightFourCone", "26-0"));
 
         // 27 Drive forward / backwards if you need to. (check with distance sensor)
+        addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1"));
 
         // 28 Bring Upper arm down on junction
+        addState(new TopArm(robot, "RightFourCone", "28-0"));
 
         // 29 Let go of cone right after arm is in position
+        addState(new CollectorState(robot, "RightFourCone", "29-0"));
 
         // 30 Raise arm as soon as the cone is dropped
+        addState(new TopArm(robot, "RightFourCone", "30-0"));
 
         // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
 
         // 32 Rotate towards Stack of cones
+        addState(new RotationState(robot, "RightFourCone", "32-0"));
 
         // 33 Decide which path after scanning image from earlier
+        addState(new PathDecision(robot, "RightFourCone", "33-0"));
 
         // 34 Drive backwards, forwards, or stay put
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-1"));
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-2"));
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-3"));
 
         // 35 Rotate towards alliance terminal
+        addState(new RotationState(robot, "RightFourCone", "35-0"));
 
     }
 

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

@@ -0,0 +1,112 @@
+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 DriverStateWithOdometerLowerArmParallelState extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private int maximumTolerance;
+    public DriverStateWithOdometerLowerArmParallelState(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.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() {
+
+        addParallelState(new BottomArm(robot, "RightFourCone", "06-1"));
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+
+        double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
+        double delta = traveledDistance - Math.abs(CurrentPosition);
+
+        if (Math.abs(CurrentPosition) <= RampUpDistance){
+            // ramping up
+           drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
+        }
+        else if (Math.abs(CurrentPosition) >= 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(CurrentPosition) < traveledDistance - maximumTolerance){
+            robot.backLeftDrive.setPower(drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+
+        }
+        else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
+            robot.backLeftDrive.setPower(targetDrivePower * -0.25);
+            robot.backRightDrive.setPower(targetDrivePower * -0.25);
+            robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
+            robot.frontRightDrive.setPower(targetDrivePower * -0.25);
+        } 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("Odometer", robot.OdometerEncoder.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);
+
+    }
+}

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

@@ -0,0 +1,112 @@
+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 DriverStateWithOdometerLowerArmParallelState2nd extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private int maximumTolerance;
+    public DriverStateWithOdometerLowerArmParallelState2nd(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.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() {
+
+        addParallelState(new BottomArm(robot, "RightFourCone", "10-1"));
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+
+        double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
+        double delta = traveledDistance - Math.abs(CurrentPosition);
+
+        if (Math.abs(CurrentPosition) <= RampUpDistance){
+            // ramping up
+           drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
+        }
+        else if (Math.abs(CurrentPosition) >= 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(CurrentPosition) < traveledDistance - maximumTolerance){
+            robot.backLeftDrive.setPower(drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+
+        }
+        else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
+            robot.backLeftDrive.setPower(targetDrivePower * -0.25);
+            robot.backRightDrive.setPower(targetDrivePower * -0.25);
+            robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
+            robot.frontRightDrive.setPower(targetDrivePower * -0.25);
+        } 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("Odometer", robot.OdometerEncoder.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);
+
+    }
+}

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

@@ -0,0 +1,112 @@
+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 DriverStateWithOdometerUpperArmParallelState extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private int maximumTolerance;
+    public DriverStateWithOdometerUpperArmParallelState(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.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() {
+
+        addParallelState(new TopArm(robot, "RightFourCone", "04-1"));
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+
+        double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
+        double delta = traveledDistance - Math.abs(CurrentPosition);
+
+        if (Math.abs(CurrentPosition) <= RampUpDistance){
+            // ramping up
+           drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
+        }
+        else if (Math.abs(CurrentPosition) >= 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(CurrentPosition) < traveledDistance - maximumTolerance){
+            robot.backLeftDrive.setPower(drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+
+        }
+        else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
+            robot.backLeftDrive.setPower(targetDrivePower * -0.25);
+            robot.backRightDrive.setPower(targetDrivePower * -0.25);
+            robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
+            robot.frontRightDrive.setPower(targetDrivePower * -0.25);
+        } 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("Odometer", robot.OdometerEncoder.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);
+
+    }
+}

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

@@ -42,7 +42,7 @@ public class JunctionAllignmentState extends CyberarmState {
                     break;
             }
                 // the state is finished if the distance sensors are at the correct distance.
-                if (leftDistance == TargetSensorDistance && rightDistance == TargetSensorDistance) {
+                if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
                     robot.frontLeftDrive.setPower(0);
                     robot.frontRightDrive.setPower(0);
                     robot.backLeftDrive.setPower(0);
@@ -77,29 +77,29 @@ public class JunctionAllignmentState extends CyberarmState {
                     robot.backLeftDrive.setPower(drivePower);
                     robot.backRightDrive.setPower(-drivePower);
                     }
-                // The right sensor is aligned but the robot must rotate rotate CW with only the left side powered
+                // The right sensor is aligned but the robot must rotate CW with only the left side powered
-                else if (leftDistance < TargetSensorDistance && rightDistance == TargetSensorDistance) {
+                else if (leftDistance < TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
                     robot.frontLeftDrive.setPower(drivePower);
                     robot.frontRightDrive.setPower(0);
                     robot.backLeftDrive.setPower(drivePower);
                     robot.backRightDrive.setPower(0);
                     }
                 // The right sensor is aligned but the robot must rotate rotate CCW with only the left side powered
-                else if (leftDistance > TargetSensorDistance && rightDistance == TargetSensorDistance) {
+                else if (leftDistance > TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
                     robot.frontLeftDrive.setPower(-drivePower);
                     robot.frontRightDrive.setPower(0);
                     robot.backLeftDrive.setPower(-drivePower);
                     robot.backRightDrive.setPower(0);
                     }
-                // The left sensor is aligned but the robot must rotate rotate CW with only the right side powered
+                // The left sensor is aligned but the robot must rotate CW with only the right side powered
-                else if (leftDistance == TargetSensorDistance && rightDistance < TargetSensorDistance) {
+                else if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance < TargetSensorDistance) {
                     robot.frontLeftDrive.setPower(0);
                     robot.frontRightDrive.setPower(-drivePower);
                     robot.backLeftDrive.setPower(0);
                     robot.backRightDrive.setPower(-drivePower);
                     }
                 // The left sensor is aligned but the robot must rotate CCW with only the right side powered
-                else if (leftDistance == TargetSensorDistance && rightDistance > TargetSensorDistance) {
+                else if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) {
                     robot.frontLeftDrive.setPower(0);
                     robot.frontRightDrive.setPower(drivePower);
                     robot.backLeftDrive.setPower(0);

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

@@ -0,0 +1,82 @@
+package org.timecrafters.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
+
+public class TopArmRotateParallelState extends CyberarmState {
+
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance;
+    long time;
+    long lastStepTime = 0;
+    boolean up;
+    boolean directPosition;
+
+    public TopArmRotateParallelState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "UpperRiserLeftPos").value();
+        this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "UpperRiserRightPos").value();
+        this.time = robot.configuration.variable(groupName, actionName, "time").value();
+        this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
+        this.directPosition = robot.configuration.variable(groupName, actionName, "directPosition").value();
+
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+
+    }
+
+    @Override
+    public void start() {
+        up = robot.HighRiserLeft.getPosition() < UpperRiserLeftPos;
+    }
+
+    @Override
+    public void exec() {
+        addParallelState(new RotationState(robot, "RightFourCone", "12-0"));
+
+        if (stateDisabled){
+            setHasFinished(true);
+            return;
+        }
+
+        if (directPosition) {
+            robot.HighRiserLeft.setPosition(UpperRiserLeftPos);
+            robot.HighRiserRight.setPosition(UpperRiserRightPos);
+
+            if (runTime() >= time){
+                setHasFinished(true);
+            }
+        } else {
+            if (System.currentTimeMillis() - lastStepTime >= time) {
+                lastStepTime = System.currentTimeMillis();
+
+                if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) {
+
+                    robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
+                    robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
+
+                } else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
+
+                    robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
+                    robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);
+
+                } else {
+                    setHasFinished(true);
+                }
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("UpperRiserRightPos",UpperRiserRightPos);
+        engine.telemetry.addData("UpperRiserLeftPos",UpperRiserLeftPos);
+        engine.telemetry.addData("AddedDistance",AddedDistance);
+        engine.telemetry.addData("time",time);
+        engine.telemetry.addData("servo position left", robot.HighRiserLeft.getPosition());
+        engine.telemetry.addData("servo position Right", robot.HighRiserRight.getPosition());
+
+
+
+    }
+}

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

@@ -19,6 +19,10 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 public class PhoenixBot1 {
 
+    private static final float mmPerInch        = 25.4f;
+    public static final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public static final int COUNTS_PER_REVOLUTION = 8192;
+
 //    private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
     private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
 
@@ -52,6 +56,14 @@ public class PhoenixBot1 {
 
          public AdafruitI2cColorSensor AdafruitEncoder;
 
+    public double ticksToInches(int ticks) {
+        return ticks * (WHEEL_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
+    }
+
+    public double inchesToTicks(double inches) {
+        return inches * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE);
+    }
+
 
         public PhoenixBot1(CyberarmEngine engine) {
             this.engine = engine;
@@ -62,6 +74,7 @@ public class PhoenixBot1 {
         }
 
         private void setupRobot () {
+
             collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
             downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
             leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");