autonomous work

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

@@ -7,13 +7,8 @@ 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.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;
@@ -50,6 +45,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
         // 6-1 drive forward or backwards if needed this is customizable so we can adapt
         addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
 
+        // 6-3 align to junction with rotation or skip if it looks like it won't be able to
+        addState(new JunctionAllignmentState(robot, "RightFourCone", "06-3"));
+
         //pause
         addState(new BottomArm(robot, "RightFourCone", "06-2"));
 

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

@@ -115,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState {
 
             distanceDelta = LastDistanceRead - currentDistance;
 
-            if (distanceDelta >= -50.0 || currentDistance > 500) {
+            if (distanceDelta >= -15.0 || currentDistance > 500) {
                 // I am moving forward
                 // and im close too my target.
                 LastDistanceRead = currentDistance;

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

@@ -1,112 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class 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

@@ -1,116 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class 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.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-
-
-    }
-
-    @Override
-    public void exec() {
-
-        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

@@ -1,144 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import android.util.Log;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TeleOp.states.PhoenixBot1;
-
-public class 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);
-
-        addParallelState(new TopArm(robot, "RightFourCone", "04-1"));
-
-    }
-
-    @Override
-    public void exec() {
-
-        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);
-        }
-        if (!getHasFinished()){
-            float angle = robot.imu.getAngularOrientation().firstAngle;
-
-            if (angle < -0.25){
-                robot.backLeftDrive.setPower(drivePower * 0.25);
-                robot.backRightDrive.setPower(drivePower * 1.25);
-                robot.frontLeftDrive.setPower(drivePower * 0.25);
-                robot.frontRightDrive.setPower(drivePower * 1.25);
-            }
-            if (angle > 0.25){
-                robot.backLeftDrive.setPower(drivePower * 1.25);
-                robot.backRightDrive.setPower(drivePower * 0.25);
-                robot.frontLeftDrive.setPower(drivePower * 1.25);
-                robot.frontRightDrive.setPower(drivePower * 0.25);
-            }
-        }
-
-
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
-        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
-        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
-        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
-        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
-        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
-        engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
-        engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
-        engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
-        engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
-
-
-        engine.telemetry.addData("drivePower", drivePower);
-        engine.telemetry.addData("targetDrivePower", targetDrivePower);
-
-        engine.telemetry.addData("traveledDistance", traveledDistance);
-        engine.telemetry.addData("RampUpDistance", RampUpDistance);
-        engine.telemetry.addData("RampDownDistance", RampDownDistance);
-
-        Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
-        Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
-        Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
-
-    }
-}

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

@@ -7,15 +7,23 @@ import org.timecrafters.TeleOp.states.PhoenixBot1;
 public class JunctionAllignmentState extends CyberarmState {
     private final boolean stateDisabled;
     PhoenixBot1 robot;
-    private double TargetSensorDistance;
-    private final String targetedJunction;
     private final double drivePower;
-    private int whatToDo;
+    private int loopsTotal;
+    private float rotationAmount;
+    private float currentAngle;
+    private float targetAngle;
+    private float slop;
+    private double checkTime = 250;
+
+
 
     public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
-        this.targetedJunction = robot.configuration.variable(groupName, actionName, "targetedJunction").value();
+        this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
+        this.rotationAmount = robot.configuration.variable(groupName, actionName, "rotationAmount").value();
+        this.slop = robot.configuration.variable(groupName, actionName, "slop").value();
+
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
     }
 
@@ -23,133 +31,132 @@ public class JunctionAllignmentState extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addData("right sensor distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
         engine.telemetry.addData("left sensor distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
+        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("target angle", targetAngle);
+        engine.telemetry.addData("current angle", currentAngle);
+        engine.telemetry.addData("drive power", drivePower);
+
+
+
 
 
+    }
+
+    @Override
+    public void start() {
+
     }
 
     @Override
     public void exec() {
 
+        currentAngle = robot.imu.getAngularOrientation().firstAngle;
+
+
         if (stateDisabled){
             setHasFinished(true);
         } else {
 
             double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
             double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM);
+            boolean rightInRange = rightDistance > 170 && rightDistance < 200;
+            boolean leftInRange = leftDistance > 170 && leftDistance < 200;
 
 
-            // TODO: 12/11/2022 Make sure these are the correct values for the distance from low, mid, and high junctions!!!
+            // The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0
-            switch (targetedJunction) {
+
-                case "low":
+            if (loopsTotal >= 5){
-                    TargetSensorDistance = 90.0;
+                setHasFinished(true);
-                    break;
+            } else if (runTime() - checkTime >= 250 ) {
-                case "mid":
+
-                    TargetSensorDistance = 135.0;
+                checkTime = runTime();
-                    break;
+                loopsTotal = loopsTotal + 1;
-                case "high":
-                    TargetSensorDistance = 200.0;
-                    break;
-            }
 
 
-                // the state is finished if the distance sensors are at the correct distance.
+            } else {
-                if ((leftDistance > TargetSensorDistance - 2.0 || leftDistance < TargetSensorDistance + 2.0) && (rightDistance > TargetSensorDistance -2.0 || rightDistance < TargetSensorDistance + 2.0)) {
+
-                    robot.frontLeftDrive.setPower(0);
+                if (rightInRange && leftInRange){
-                    robot.frontRightDrive.setPower(0);
-                    robot.backLeftDrive.setPower(0);
-                    robot.backRightDrive.setPower(0);
                     setHasFinished(true);
+                }
+
+                if (rightInRange && !leftInRange) {
+
+                    // rotate 1 degree CW
+
+                    targetAngle = currentAngle + rotationAmount;
+
+                    if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) {
+
+                        robot.frontRightDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+
+                    } else {
+
+                        if (targetAngle - slop < currentAngle) {
+
+                            robot.frontRightDrive.setPower(-drivePower);
+                            robot.backRightDrive.setPower(-drivePower);
+                            robot.backLeftDrive.setPower(drivePower);
+                            robot.frontLeftDrive.setPower(drivePower);
+
+                        } else if (targetAngle + slop > currentAngle) {
+
+                            robot.frontRightDrive.setPower(drivePower);
+                            robot.backRightDrive.setPower(drivePower);
+                            robot.backLeftDrive.setPower(-drivePower);
+                            robot.frontLeftDrive.setPower(-drivePower);
+
+                        }
+
                     }
 
-//                whatToDo = (int)((leftDistance > TargetSensorDistance) << 1 ) + (int)(rightDistance > TargetSensorDistance);
-
-                switch (whatToDo){
-                    case 0: // drive back
-                        robot.frontLeftDrive.setPower(-drivePower);
-                        robot.frontRightDrive.setPower(-drivePower);
-                        robot.backLeftDrive.setPower(-drivePower);
-                        robot.backRightDrive.setPower(-drivePower);
-                        break;
-
-                    case 1: // rotate CW
-                        robot.frontLeftDrive.setPower(drivePower);
-                        robot.frontRightDrive.setPower(-drivePower);
-                        robot.backLeftDrive.setPower(drivePower);
-                        robot.backRightDrive.setPower(-drivePower);
-                        break;
-
-                    case 2: // rotate CCW
-                        robot.frontLeftDrive.setPower(-drivePower);
-                        robot.frontRightDrive.setPower(drivePower);
-                        robot.backLeftDrive.setPower(-drivePower);
-                        robot.backRightDrive.setPower(drivePower);
-                        break;
-
-                    case 3: // Drive Forward
-                        robot.frontLeftDrive.setPower(drivePower);
-                        robot.frontRightDrive.setPower(drivePower);
-                        robot.backLeftDrive.setPower(drivePower);
-                        robot.backRightDrive.setPower(drivePower);
-                        break;
-
-
                 }
-                // the robot is lined up but needs to drive forward till the robot is at the specified distance
+                if (!rightInRange && leftInRange) {
-                if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){
+
-                    robot.frontLeftDrive.setPower(drivePower);
+                    // rotate 1 degree CCW
-                    robot.frontRightDrive.setPower(drivePower);
+
-                    robot.backLeftDrive.setPower(drivePower);
+                    targetAngle = currentAngle - rotationAmount;
-                    robot.backRightDrive.setPower(drivePower);
+
+                    if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) {
+
+                        robot.frontRightDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+
+                    } else {
+
+                        if (targetAngle - slop < currentAngle) {
+
+                            robot.frontRightDrive.setPower(-drivePower);
+                            robot.backRightDrive.setPower(-drivePower);
+                            robot.backLeftDrive.setPower(drivePower);
+                            robot.frontLeftDrive.setPower(drivePower);
+
+                        } else if (targetAngle + slop > currentAngle) {
+
+                            robot.frontRightDrive.setPower(drivePower);
+                            robot.backRightDrive.setPower(drivePower);
+                            robot.backLeftDrive.setPower(-drivePower);
+                            robot.frontLeftDrive.setPower(-drivePower);
+
+                        }
+
                     }
-                // the robot is lined up but needs to drive backward till the robot is at the specified distance
+
-                if (leftDistance < TargetSensorDistance && rightDistance < TargetSensorDistance){
+                }
-                    robot.frontLeftDrive.setPower(-drivePower);
+
-                    robot.frontRightDrive.setPower(-drivePower);
+                if (!rightInRange && !leftInRange){
-                    robot.backLeftDrive.setPower(-drivePower);
+
-                    robot.backRightDrive.setPower(-drivePower);
+                    setHasFinished(true);
-                    }
+
-                // the robot is going to rotate CCW until a distance is met
-                if (leftDistance > TargetSensorDistance && rightDistance < TargetSensorDistance) {
-                    robot.frontLeftDrive.setPower(-drivePower);
-                    robot.frontRightDrive.setPower(drivePower);
-                    robot.backLeftDrive.setPower(-drivePower);
-                    robot.backRightDrive.setPower(drivePower);
-                    }
-                // the robot is going to rotate CW until a distance is met
-                if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) {
-                    robot.frontLeftDrive.setPower(drivePower);
-                    robot.frontRightDrive.setPower(-drivePower);
-                    robot.backLeftDrive.setPower(drivePower);
-                    robot.backRightDrive.setPower(-drivePower);
-                    }
-                // The right sensor is aligned but the robot must rotate CW with only the left side powered
-                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
-                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 CW with only the right side powered
-                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
-                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);
                     }
+                }
+            }
         }
     }
-}

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

@@ -81,6 +81,10 @@ public class RotationState extends CyberarmState {
         engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
         engine.telemetry.addData("Robot Target Rotation", targetRotation);
         engine.telemetry.addData("Drive Power", drivePowerVariable);
+        engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
+        engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
 
     }
 }