RightSideAutonomous and leftsideAutonomous work added

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

@@ -0,0 +1,96 @@
+package org.timecrafters.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+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.TopArm;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+@Autonomous (name = "Left Side")
+
+public class LeftSideAutonomousEngine extends CyberarmEngine {
+    PrototypeBot1 robot;
+
+    @Override
+    public void setup() {
+        robot = new PrototypeBot1(this);
+        addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-0"));
+        //drive to high pole
+        addState(new DriverState(robot, "LeftSideAutonomous", "01-0"));
+        //turn towards high pole
+        addState(new RotationState(robot, "LeftSideAutonomous", "02-0"));
+        //lift the upper arm
+        addState(new TopArm(robot, "LeftSideAutonomous", "03-0"));
+        //lift the lower arm
+        addState(new BottomArm(robot, "LeftSideAutonomous", "04-0"));
+        //drive forward
+        addState(new DriverState(robot, "LeftSideAutonomous", "05-0"));
+        //lower the bottom arm to get closer
+        addState(new TopArm(robot, "LeftSideAutonomous", "06-0"));
+        //make collector release the cone
+        addState(new CollectorState(robot, "LeftSideAutonomous", "07-0"));
+        //lift the lower arm to clear the pole
+        addState(new TopArm(robot, "LeftSideAutonomous", "08-0"));
+        //Drive Backwards
+        addState(new DriverState(robot, "LeftSideAutonomous", "09-0"));
+        // Rotate to either set up for cones to grab or park somewhere
+        addState(new RotationState(robot, "LeftSideAutonomous", "12-0"));
+        // lower the bottom arm so we dont fall over
+        addState(new BottomArm(robot, "LeftSideAutonomous", "10-0"));
+        // lower the upper arm so we dont fall over
+        addState(new TopArm(robot, "LeftSideAutonomous", "11-0"));;
+        //adjust arm height to cone.
+        addState(new TopArm(robot, "LeftSideAutonomous", "13-0"));
+        //drive towards stack of cones while collecting
+        addState(new CollectorDistanceState(robot, "LeftSideAutonomous", "14-0"));
+        //drive slightly back
+        addState(new DriverState(robot, "LeftSideAutonomous", "15-0"));
+        // face to -90
+        addState(new RotationState(robot, "LeftSideAutonomous", "15-1"));
+        //lift arm up
+        addState(new TopArm(robot, "LeftSideAutonomous", "16-0"));
+        // drive backwards too position
+        addState(new DriverState(robot, "LeftSideAutonomous", "17-0"));
+        // rotate
+        addState(new RotationState(robot, "LeftSideAutonomous", "18-0"));
+
+        addState(new DriverState(robot, "LeftSideAutonomous", "21-0"));
+        // bring arm down.
+        addState(new TopArm(robot, "LeftSideAutonomous", "22-0"));
+        // get rid of cone
+        addState(new CollectorState(robot, "LeftSideAutonomous", "23-0"));
+        // lift arm up to clear
+        addState(new TopArm(robot, "LeftSideAutonomous", "24-0"));
+        // rotate towards stack of cones
+        addState(new RotationState(robot, "LeftSideAutonomous", "28-0"));
+        // Choose Parking Spot
+        addState(new PathDecision(robot, "LeftSideAutonomous", "29-0"));
+        // case 1 drive forward
+        addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-1"));
+        // case 2 drive forward
+        addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-2"));
+        // case 3 drive forward
+        addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-3"));
+        // zero out
+        addState(new RotationState(robot, "LeftSideAutonomous", "30-0"));
+        // Top Arm Down
+        addState(new TopArm(robot, "LeftSideAutonomous", "28-1"));
+        //Drive Backwards
+        addState(new DriverState(robot, "LeftSideAutonomous", "30-1"));
+    }
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
+    }
+}

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

@@ -0,0 +1,94 @@
+package org.timecrafters.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+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.BottomArm;
+import org.timecrafters.Autonomous.States.PathDecision;
+import org.timecrafters.Autonomous.States.RotationState;
+import org.timecrafters.Autonomous.States.TopArm;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+@Autonomous (name = "Right Side")
+
+public class RightSideAutonomousEngine extends CyberarmEngine {
+    PrototypeBot1 robot;
+
+    @Override
+    public void setup() {
+        robot = new PrototypeBot1(this);
+        addState(new ConeIdentification(robot, "RightSideAutonomous", "00-0"));
+        //drive to high pole
+        addState(new DriverState(robot, "RightSideAutonomous", "01-0"));
+        //turn towards high pole
+        addState(new RotationState(robot, "RightSideAutonomous", "02-0"));
+        //lift the upper arm
+        addState(new TopArm(robot, "RightSideAutonomous", "03-0"));
+        //lift the lower arm
+        addState(new BottomArm(robot, "RightSideAutonomous", "04-0"));
+        //drive forward
+        addState(new DriverState(robot, "RightSideAutonomous", "05-0"));
+        //lower the bottom arm to get closer
+        addState(new TopArm(robot, "RightSideAutonomous", "06-0"));
+        //make collector release the cone
+        addState(new CollectorState(robot, "RightSideAutonomous", "07-0"));
+        //lift the lower arm to clear the pole
+        addState(new TopArm(robot, "RightSideAutonomous", "08-0"));
+        //Drive Backwards
+        addState(new DriverState(robot, "RightSideAutonomous", "09-0"));
+        // lower the bottom arm so we dont fall over
+        addState(new BottomArm(robot, "RightSideAutonomous", "10-0"));
+        // lower the upper arm so we dont fall over
+        addState(new TopArm(robot, "RightSideAutonomous", "11-0"));;
+        // Rotate to either set up for cones to grab or park somewhere
+        addState(new RotationState(robot, "RightSideAutonomous", "12-0"));
+        //adjust arm height to cone.
+        addState(new TopArm(robot, "RightSideAutonomous", "13-0"));
+        //drive towards stack of cones while collecting
+        addState(new CollectorDistanceState(robot, "RightSideAutonomous", "14-0"));
+        //drive slightly back
+        addState(new DriverState(robot, "RightSideAutonomous", "15-0"));
+        // face to -90
+        addState(new RotationState(robot, "RightSideAutonomous", "15-1"));
+        //lift arm up
+        addState(new TopArm(robot, "RightSideAutonomous", "16-0"));
+        // drive backwards too position
+        addState(new DriverState(robot, "RightSideAutonomous", "17-0"));
+        // rotate
+        addState(new RotationState(robot, "RightSideAutonomous", "18-0"));
+
+        addState(new DriverState(robot, "RightSideAutonomous", "21-0"));
+        // bring arm down.
+        addState(new TopArm(robot, "RightSideAutonomous", "22-0"));
+        // get rid of cone
+        addState(new CollectorState(robot, "RightSideAutonomous", "23-0"));
+        // lift arm up to clear
+        addState(new TopArm(robot, "RightSideAutonomous", "24-0"));
+        // rotate towards stack of cones
+        addState(new RotationState(robot, "RightSideAutonomous", "28-0"));
+        // Choose Parking Spot
+        addState(new PathDecision(robot, "RightSideAutonomous", "29-0"));
+        // case 1 drive forward
+        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-1"));
+        // case 2 drive forward
+        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-2"));
+        // case 3 drive forward
+        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-3"));
+        // zero out
+        addState(new RotationState(robot, "RightSideAutonomous", "30-0"));
+        // Top Arm Down
+        addState(new TopArm(robot, "RightSideAutonomous", "28-1"));
+    }
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
+    }
+}

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

@@ -1,89 +0,0 @@
-package org.timecrafters.Autonomous.Engines;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.States.CollectorDistanceState;
-import org.timecrafters.Autonomous.States.CollectorState;
-import org.timecrafters.Autonomous.States.ConeIdentification;
-import org.timecrafters.Autonomous.States.DriverState;
-import org.timecrafters.Autonomous.States.BottomArm;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.testing.states.PrototypeBot1;
-
-@Autonomous (name = "Autonomous Test")
-
-public class TestAutonomousEngine extends CyberarmEngine {
-    PrototypeBot1 robot;
-
-    @Override
-    public void setup() {
-        robot = new PrototypeBot1(this);
-        addState(new ConeIdentification(robot, "TestAutonomous", "00-0"));
-        //drive to high pole
-        addState(new DriverState(robot, "TestAutonomous", "01-0"));
-        //turn towards high pole
-        addState(new RotationState(robot, "TestAutonomous", "02-0"));
-        //lift the upper arm
-        addState(new TopArm(robot, "TestAutonomous", "03-0"));
-        //lift the lower arm
-        addState(new BottomArm(robot, "TestAutonomous", "04-0"));
-        //drive forward
-        addState(new DriverState(robot, "TestAutonomous", "05-0"));
-        //lower the bottom arm to get closer
-        addState(new TopArm(robot, "TestAutonomous", "06-0"));
-        //make collector release the cone
-        addState(new CollectorState(robot, "TestAutonomous", "07-0"));
-        //lift the lower arm to clear the pole
-        addState(new TopArm(robot, "TestAutonomous", "08-0"));
-        //Drive Backwards
-        addState(new DriverState(robot, "TestAutonomous", "09-0"));
-        // lower the bottom arm so we dont fall over
-        addState(new BottomArm(robot, "TestAutonomous", "10-0"));
-        // lower the upper arm so we dont fall over
-        addState(new TopArm(robot, "TestAutonomous", "11-0"));;
-        // Rotate to either set up for cones to grab or park somewhere
-        addState(new RotationState(robot, "TestAutonomous", "12-0"));
-        //adjust arm height to cone.
-        addState(new TopArm(robot, "TestAutonomous", "13-0"));
-        //drive towards stack of cones while collecting
-        addState(new CollectorDistanceState(robot, "TestAutonomous", "14-0"));
-        //drive slightly back
-        addState(new DriverState(robot, "TestAutonomous", "15-0"));
-        //lift arm up
-        addState(new TopArm(robot, "TestAutonomous", "16-0"));
-        // drive backwards too position
-        addState(new DriverState(robot, "TestAutonomous", "17-0"));
-        // rotate
-        addState(new RotationState(robot, "TestAutonomous", "18-0"));
-        //lift the upper arm
-        addState(new TopArm(robot, "TestAutonomous", "19-0"));
-        //lift the lower arm
-        addState(new BottomArm(robot, "TestAutonomous", "20-0"));
-        //drive forward to allign
-        addState(new DriverState(robot, "TestAutonomous", "21-0"));
-        // bring arm down.
-        addState(new TopArm(robot, "TestAutonomous", "22-0"));
-        // get rid of cone
-        addState(new CollectorState(robot, "TestAutonomous", "23-0"));
-        // lift arm up to clear
-        addState(new TopArm(robot, "TestAutonomous", "24-0"));
-        // drive back
-        addState(new DriverState(robot, "TestAutonomous", "25-0"));
-        // bring bottom arm down
-        addState(new BottomArm(robot, "TestAutonomous", "26-0"));
-        // bring top arm down
-        addState(new TopArm(robot, "TestAutonomous", "27-0"));
-        // rotate towards stack of cones
-        addState(new RotationState(robot, "TestAutonomous", "28-0"));
-    }
-
-    @Override
-    public void loop() {
-        super.loop();
-
-        telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
-    }
-}

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

@@ -102,45 +102,46 @@ public class CollectorDistanceState extends CyberarmState {
                 // and im close too my target.
                 LastDistanceRead = currentDistance;
                 debugRunTime = runTime();
-                debugStatus = "RUNNING";
+                debugStatus = "Driving Towards Cone";
             } else {
                 // I have stopped
-                debugStatus = "STOPPED";
+                debugStatus = "Nothing Collected";
+                robot.collectorLeft.setPower(0);
+                robot.collectorRight.setPower(0);
                 setHasFinished(true);
                 return;
             }
         }
 
-    if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
+        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
-        double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
+            double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
 
-        if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){
+            if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
-            // ramping up
+                // ramping up
-            drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
+                drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
-        }
+            } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
-        else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){
+                // ramping down
-            // ramping down
+                drivePower = ((delta / RampDownDistance) + 0.15);
-            drivePower = ((delta / RampDownDistance) + 0.15);
+            } else {
-        } else {
+                // middle ground
-            // middle ground
+                drivePower = targetDrivePower;
-            drivePower = targetDrivePower;
+            }
-        }
 
-        if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
+            if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
-            // This is limiting drive power to the targeted drive power
+                // This is limiting drive power to the targeted drive power
-            drivePower = targetDrivePower;
+                drivePower = targetDrivePower;
-        }
+            }
 
-        if (targetDrivePower < 0 && drivePower != targetDrivePower) {
+            if (targetDrivePower < 0 && drivePower != targetDrivePower) {
-            drivePower = drivePower * -1;
+                drivePower = drivePower * -1;
-        }
+            }
 
-        if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){
+            if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
-            robot.backLeftDrive.setPower(drivePower);
+                robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(drivePower);
-        }
+            }
     } else {
         robot.frontRightDrive.setPower(0);
         robot.frontLeftDrive.setPower(0);
@@ -150,10 +151,9 @@ public class CollectorDistanceState extends CyberarmState {
         robot.collectorLeft.setPower(0);
         setHasFinished(true);
 
-    }
+            }
+
+        }
 
     }
 
-
-
-}

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

@@ -0,0 +1,108 @@
+package org.timecrafters.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+public class DriverParkPlaceState extends CyberarmState {
+    private final boolean stateDisabled;
+    PrototypeBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private String intendedPlacement;
+
+    public DriverParkPlaceState(PrototypeBot1 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.blackboard.get("parkPlace");
+        if (placement != null) {
+            if (!placement.equals(intendedPlacement)){
+                    setHasFinished(true);
+            }
+            if (placement.equals(intendedPlacement)) {
+                double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
+
+                if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
+                    // ramping up
+                    drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
+                } else if (Math.abs(robot.frontRightDrive.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.frontRightDrive.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("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/PathDecision.java

@@ -17,17 +17,7 @@ public class PathDecision extends CyberarmState {
     @Override
     public void exec() {
         String placement = engine.blackboard.get("parkPlace");
-
+        setHasFinished(true);
-        if (placement != null) {
-            if (placement.equals("2")) {
-                engine.insertState(this, new DriverState(robot, groupName, "29-1"));
-            } else if (placement.equals("3")) {
-                engine.insertState(this, new DriverState(robot, groupName, "29-2"));
-            }
-        }
-          else {
-              engine.insertState(this, new DriverState(robot, groupName, "29-0"));
-          }
         }
 
     }

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

@@ -8,8 +8,9 @@ public class RotationState extends CyberarmState {
     PrototypeBot1 robot;
     public RotationState(PrototypeBot1 robot, String groupName, String actionName) {
         this.robot = robot;
-        this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
+        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;
 
 
@@ -18,46 +19,41 @@ public class RotationState extends CyberarmState {
     private double drivePower;
     private float targetRotation;
     float RobotRotation;
-    private double RotationTarget, DeltaRotation;
+    private double RotationTarget;
     private double RotationDirectionMinimum;
+    private String debugStatus = "?";
+    private double drivePowerVariable;
 
 
-    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);
-        }
-    }
-
     @Override
     public void exec() {
         if (stateDisabled){
             setHasFinished(true);
             return;
-        }
+            } // end of if
 
         RobotRotation = robot.imu.getAngularOrientation().firstAngle;
-        RotationTarget = targetRotation;
+
-        CalculateDeltaRotation();
+        if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
-        if (drivePower < 0){
+            drivePowerVariable = 0.3 * drivePower;
-            RotationDirectionMinimum = -0.3;
+            debugStatus = "Rotate Slow";
-        } else {
+        } // end of if
-            RotationDirectionMinimum = 0.3;
+        else {
+            drivePowerVariable = drivePower * 0.75;
+            debugStatus = "Rotate Fast";
         }
-        drivePower = (drivePower * DeltaRotation/180) + RotationDirectionMinimum;
+
-        if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) {
+        if (RobotRotation >= targetRotation + 1){
-            robot.backLeftDrive.setPower(-drivePower);
+            drivePowerVariable = Math.abs(drivePowerVariable);
-            robot.backRightDrive.setPower(drivePower);
+        } else {
-            robot.frontLeftDrive.setPower(-drivePower);
+            drivePowerVariable = -1 * Math.abs(drivePowerVariable);
-            robot.frontRightDrive.setPower(drivePower);
+        }
+
+        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);
@@ -71,9 +67,13 @@ public class RotationState extends CyberarmState {
 
     @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", drivePower);
+        engine.telemetry.addData("Drive Power", drivePowerVariable);
 
     }
 }