Restructured TeleOp

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java

@@ -15,6 +15,7 @@ public class FindWobbleGoal extends CyberarmState {
     private double range;
     private boolean cCheckPrev;
     private boolean ccCheckPrev;
+    private float startRotation;
 
     public FindWobbleGoal(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -33,6 +34,7 @@ public class FindWobbleGoal extends CyberarmState {
     @Override
     public void start() {
         setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
+        startRotation = robot.getRotation();
         robot.setDrivePower(power,-power,power,-power);
     }
 
@@ -42,8 +44,10 @@ public class FindWobbleGoal extends CyberarmState {
         double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
 
         if (sensorValue > turnCheck) {
-            boolean cCheck = robot.getRotation() > range;
-            boolean ccCheck = robot.getRotation() < -range;
+            float rotation = robot.getRelativeAngle(startRotation,robot.getRotation());
+
+            boolean cCheck = rotation > range;
+            boolean ccCheck = rotation < -range;
 
             if (cCheck && !cCheckPrev) {
                 robot.setDrivePower(-power, power, -power, power);

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java

@@ -11,6 +11,7 @@ public class Launch extends CyberarmState {
     boolean detectedPass;
     private boolean reduceConditionPrev;
     private double reducePos;
+    private long stuckStartTime;
 
     public Launch(Robot robot) {
         this.robot = robot;
@@ -39,7 +40,18 @@ public class Launch extends CyberarmState {
     public void exec() {
         //detect when limit switch is initially triggered
         boolean detectingPass = robot.limitSwitch.isPressed();
-        int beltPos = robot.getBeltPos();
+        int beltPos = robot.ringBeltMotor.getCurrentPosition();
+
+        if (robot.beltIsStuck() && childrenHaveFinished()) {
+            long currentTime = System.currentTimeMillis();
+            if (stuckStartTime == 0) {
+                stuckStartTime = currentTime;
+            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
+                addParallelState(new UnstickRingBelt(robot));
+            }
+        } else {
+            stuckStartTime = 0;
+        }
 
         if (detectingPass && !detectedPass) {
             //finish once the ring belt has cycled all the way through and then returned to
@@ -61,7 +73,7 @@ public class Launch extends CyberarmState {
         }
         detectedPass = detectingPass;
 
-        boolean reduceCondition = (hasCycled && beltPos > reducePos && beltPos < reducePos + Robot.RING_BELT_GAP);
+        boolean reduceCondition = (hasCycled && beltPos > reducePos);
         if (reduceCondition && !reduceConditionPrev){
             robot.ringBeltOn();
             //the ring belt stage lets other states know that the robot has finished launching all three rings

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java

@@ -7,6 +7,7 @@ public class ProgressRingBelt extends CyberarmState {
     private Robot robot;
     private int targetPos;
     private boolean prepLaunch;
+    private long stuckStartTime;
 
     public ProgressRingBelt(Robot robot) {
         this.robot = robot;
@@ -14,7 +15,7 @@ public class ProgressRingBelt extends CyberarmState {
 
     @Override
     public void start() {
-        int currentPos = robot.getBeltPos();
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
         if (robot.ringBeltStage < 2) {
             targetPos = robot.loopPos(currentPos + Robot.RING_BELT_GAP);
             robot.ringBeltOn();
@@ -33,8 +34,19 @@ public class ProgressRingBelt extends CyberarmState {
 
     @Override
     public void exec() {
-        int currentPos = robot.getBeltPos();
-        if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) {
+        if (robot.beltIsStuck() && childrenHaveFinished()) {
+            long currentTime = System.currentTimeMillis();
+            if (stuckStartTime == 0) {
+                stuckStartTime = currentTime;
+            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
+                addParallelState(new UnstickRingBelt(robot));
+            }
+        } else {
+            stuckStartTime = 0;
+        }
+
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+        if (currentPos >= targetPos) {
             robot.ringBeltMotor.setPower(0);
 
             if(prepLaunch) {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java

@@ -96,6 +96,7 @@ public class Robot {
     private float rotationPrevious = 0;
     public float angularVelocity;
 
+
     //Launcher
     public DcMotor launchMotor;
     public static final double LAUNCH_POWER = 0.715;
@@ -122,8 +123,11 @@ public class Robot {
     public int ringBeltStage;
     public static final int RING_BELT_LOOP_TICKS = 2544;
     public static final int RING_BELT_GAP = 670;
-
     public static final double RING_BELT_POWER = 0.2;
+    private int ringBeltPrev;
+    public long beltMaxStopTime;
+    public int beltReverseTicks;
+    public int beltMaxStopTicks;
 
     //Wobble Goal Arm
     public DcMotor wobbleArmMotor;
@@ -212,7 +216,9 @@ public class Robot {
         ringBeltMotor = hardwareMap.dcMotor.get("belt");
         ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- 
+        beltMaxStopTime = stateConfiguration.variable("system","belt", "maxStopTime").value();
+        beltMaxStopTicks = stateConfiguration.variable("system","belt", "maxStopTicks").value();
+        beltReverseTicks = stateConfiguration.variable("system","belt", "reverseTicks").value();
 
         //init IMU
         imu  = hardwareMap.get(BNO055IMU.class, "imu");
@@ -615,8 +621,11 @@ public class Robot {
         ringBeltMotor.setPower(RING_BELT_POWER);
     }
 
-    public int getBeltPos(){
-        return loopPos(ringBeltMotor.getCurrentPosition());
+    public boolean beltIsStuck() {
+        int ringBeltPos = ringBeltMotor.getCurrentPosition();
+        boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks);
+        ringBeltPrev = ringBeltPos;
+        return notMoved;
     }
 
     public int loopPos(int pos) {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java

@@ -0,0 +1,168 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Player1 extends CyberarmState {
+    private Robot robot;
+
+    //normal drive control
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double rightJoystickMagnitudePrevious;
+
+    private double faceControlThreshold;
+    private float cardinalSnapping;
+    private float pairSnapping;
+
+    private float faceDirection = 0;
+    private double[] powers = {0,0,0,0};
+    private double drivePower = 1;
+    private boolean lbPrev;
+
+    //find wobble goal control
+    private FindWobbleGoal findWobbleGoal;
+    private boolean runNextFindWobble;
+    private boolean findWobbleInputPrev;
+
+    //Drive to launch control
+    private DriveToCoordinates driveToLaunch;
+    private boolean runNextDriveToLaunch;
+    private boolean driveToLaunchInputPrev;
+
+    private double launchTolerance;
+    private double launchPower;
+    private long launchBrakeTime;
+
+    public Player1(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
+        pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
+        faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
+
+        launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
+        launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
+        launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        boolean lb = engine.gamepad1.left_stick_button;
+        if (lb && !lbPrev) {
+            if (drivePower == 1) {
+                drivePower = 0.5;
+            } else {
+                drivePower = 1;
+            }
+        }
+        lbPrev = lb;
+
+        runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
+
+        boolean findWobbleInput = engine.gamepad1.x;
+        if (findWobbleInput) {
+            if (runNextFindWobble && !findWobbleInputPrev) {
+                findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
+                addParallelState(findWobbleGoal);
+            }
+            faceDirection = robot.getRotation();
+        } else if (!runNextFindWobble) {
+            findWobbleGoal.setHasFinished(true);
+        }
+        findWobbleInputPrev = findWobbleInput;
+
+
+        runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished());
+
+        boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput;
+        if (driveToLaunchInput) {
+            if (runNextDriveToLaunch && !driveToLaunchInputPrev) {
+                driveToLaunch = new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime);
+                addParallelState(driveToLaunch);
+            }
+            faceDirection = robot.getRotation();
+        } else if (!runNextDriveToLaunch) {
+            driveToLaunch.setHasFinished(true);
+        }
+        driveToLaunchInputPrev = driveToLaunchInput;
+
+        if (childrenHaveFinished()) {
+            //Normal Driver Controls
+
+            double leftJoystickX = engine.gamepad1.left_stick_x;
+            double leftJoystickY = engine.gamepad1.left_stick_y;
+
+            leftJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
+            leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+            double rightJoystickX = engine.gamepad1.right_stick_x;
+            double rightJoystickY = engine.gamepad1.right_stick_y;
+
+            rightJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
+            rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+            //if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
+            if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
+
+                //if the joystick is close to one of the cardinal directions, it is set to exactly the cardinal direction
+                faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0);
+            }
+            rightJoystickMagnitudePrevious = rightJoystickMagnitude;
+
+            //allows the the driver to indicate which direction the robot is currently looking so
+            //so that the controller and robot can be re-synced in the event of a bad initial
+            //position.
+            if (engine.gamepad1.right_stick_button) {
+                robot.resetRotation(faceDirection);
+            }
+
+            if (leftJoystickMagnitude == 0) {
+                double[] facePowers = robot.getFacePowers(faceDirection, drivePower);
+                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
+            } else {
+                //drives the robot in the direction of the move joystick while facing the direction
+                //of the look joystick. if the move direction is almost aligned/perpendicular to the
+                //look joystick,
+                powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), drivePower, faceDirection);
+            }
+
+            robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
+        }
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Player 1 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+    private float snapToCardinal(float angle, float snapping, float offset) {
+        int o = (int) offset + 180;
+        o %= 90;
+        for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) {
+            if (angle >= cardinal-snapping && angle <= cardinal+snapping) {
+                angle = cardinal;
+            }
+        }
+        return angle;
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java

@@ -0,0 +1,136 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Player2 extends CyberarmState {
+    private Robot robot;
+
+    private boolean rbPrev;
+    private boolean yPrev;
+    private boolean xPrev;
+    private boolean bPrev;
+    private boolean wobbleArmUp = false;
+    private boolean wobbleGrabOpen = false;
+    private boolean aPrev;
+    private double beltPowerPrev;
+    private boolean lbPrev;
+    private boolean manualArmHold;
+
+    private boolean launchInput = false;
+
+    public Player2(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void init() {
+        robot.wobbleArmMotor.setTargetPosition(0);
+        robot.wobbleArmMotor.setPower(0.5);
+    }
+
+    @Override
+    public void exec() {
+
+        //Collector control
+        if (childrenHaveFinished()) {
+            robot.collectionMotor.setPower(engine.gamepad2.right_trigger);
+        } else {
+            robot.collectionMotor.setPower(0);
+        }
+
+        //belt progression control
+        boolean rb = engine.gamepad2.right_bumper;
+        if (rb && !rbPrev && childrenHaveFinished()) {
+            addParallelState(new ProgressRingBelt(robot));
+        }
+        rbPrev = rb;
+
+        //launch sequence control
+        boolean y2 = engine.gamepad2.y;
+        if (y2 && !yPrev && childrenHaveFinished()) {
+            addParallelState(new Launch(robot));
+        }
+        yPrev = y2;
+
+        //toggles wobble grabber open and closed
+        boolean x = engine.gamepad2.x;
+        if (x && !xPrev) {
+            wobbleGrabOpen = !wobbleGrabOpen;
+            if (wobbleGrabOpen) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+            } else {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.05 );
+            }
+        }
+        xPrev = x;
+
+        //toggles the wobble arm up and down.
+        boolean b = engine.gamepad2.b;
+        if (b && !bPrev) {
+            wobbleArmUp = !wobbleArmUp;
+            if (wobbleArmUp) {
+                robot.wobbleArmMotor.setTargetPosition(550);
+            } else {
+                robot.wobbleArmMotor.setTargetPosition(0);
+            }
+        }
+        bPrev = b;
+
+        //manually toggle the launch wheel for emergencies
+        boolean a = engine.gamepad2.a;
+        if (a && !aPrev) {
+            if (robot.launchMotor.getPower() == 0) {
+                robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+            } else {
+                robot.launchMotor.setPower(0);
+            }
+        }
+        aPrev = a;
+
+        //manually control the wobble arm for when it's initialized in an unexpected position.
+        if (engine.gamepad2.dpad_up) {
+            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.wobbleArmMotor.setPower(0.5);
+            manualArmHold = true;
+        } else if (engine.gamepad2.dpad_down) {
+            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.wobbleArmMotor.setPower(-0.1);
+            manualArmHold = true;
+        } else if (manualArmHold) {
+            manualArmHold = false;
+            robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
+            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        }
+
+        //allows the driver to revers the belt in the event of a jam
+        boolean lb = engine.gamepad2.left_bumper;
+        if (lb && !lbPrev) {
+            beltPowerPrev = robot.ringBeltMotor.getPower();
+            robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
+        }
+
+        if (!lb && lbPrev) {
+            robot.ringBeltMotor.setPower(beltPowerPrev);
+        }
+
+        lbPrev = lb;
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Player 2 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java

@@ -3,241 +3,34 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
 import org.timecrafters.UltimateGoal.Competition.Launch;
 import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
 import org.timecrafters.UltimateGoal.Competition.Robot;
-import org.timecrafters.UltimateGoal.Competition.WobbleArm;
-import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
 
 public class TeleOpState extends CyberarmState {
 
     private Robot robot;
-    private float leftJoystickDegrees;
-    private double leftJoystickMagnitude;
-    private float rightJoystickDegrees;
-    private double rightJoystickMagnitude;
-    private double rightJoystickMagnitudePrevious;
-
-    private float faceDirection = 0;
-
-    private double faceControlThreshold;
-    private float cardinalSnapping;
-    private float pairSnapping;
-
-
-
-    private double[] powers = {0,0,0,0};
-    private double drivePower = 1;
-    private static final double TURN_POWER = 1;
-    private static final double LAUNCH_TOLERANCE_FACE = 1;
-    private Launch launchState;
-    private boolean launching;
-    private ProgressRingBelt ringBeltState;
-    private boolean CollectorOn;
-    private boolean rbPrev;
-    private boolean yPrev;
-    private boolean xPrev;
-    private boolean bPrev;
-    private boolean lbPrev;
-    private boolean wobbleArmUp = false;
-    private boolean wobbleGrabOpen = false;
-    private boolean emergencyLaunchPrev;
-    private double beltPowerPrev;
-    private boolean reverseBeltPrev;
-    private int manualArmHoldPos;
-    private boolean manualArmHold;
-
-    private boolean launchInput = false;
-
 
     public TeleOpState(Robot robot) {
         this.robot = robot;
     }
 
-    private double LAUNCH_TOLERANCE_POS;
-
     @Override
-    public void init() {
-        cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
-        pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
-        faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
-        robot.wobbleArmMotor.setTargetPosition(0);
-        robot.wobbleArmMotor.setPower(0.5);
-        LAUNCH_TOLERANCE_POS = robot.inchesToTicks(3);
-        robot.resetRotation(180);
+    public void start() {
+        addParallelState(new Player1(robot));
+        addParallelState(new Player2(robot));
     }
 
     @Override
     public void exec() {
-        robot.updateLocation();
-        robot.record(""+robot.ticksToInches(robot.getLocationX())+", "+robot.ticksToInches(robot.getLocationY()));
-
-        boolean lb = engine.gamepad1.left_stick_button;
-        if (lb && !lbPrev) {
-            if (drivePower == 1) {
-                drivePower = 0.5;
-            } else {
-                drivePower = 1;
-            }
-        }
-        lbPrev = lb;
-
-        double[] powers = {0,0,0,0};
-
-        boolean y = engine.gamepad1.y;
-
-        if (y) {
-
-            //Drive to Launch Pos
-            double distanceToTarget = Math.hypot(robot.launchPositionX - robot.getLocationX(), robot.launchPositionY - robot.getLocationY());
-            if (distanceToTarget > LAUNCH_TOLERANCE_POS) {
-                powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.launchPositionX, robot.launchPositionY), drivePower, robot.launchRotation);
-
-            } else if (Math.abs(robot.getRelativeAngle(robot.launchRotation,robot.getRotation())) > LAUNCH_TOLERANCE_FACE) {
-                double[] facePowers =  robot.getFacePowers(robot.launchRotation, TURN_POWER);
-                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
-            } else {
-
-            }
-
-        } else {
-            //Normal Driver Controls
-
-            double leftJoystickX = engine.gamepad1.left_stick_x;
-            double leftJoystickY = engine.gamepad1.left_stick_y;
-
-            leftJoystickDegrees = robot.getRelativeAngle(90,  (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
-            leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
-
-            double rightJoystickX = engine.gamepad1.right_stick_x;
-            double rightJoystickY = engine.gamepad1.right_stick_y;
-
-            rightJoystickDegrees = robot.getRelativeAngle (90, (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
-            rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
-
-            //if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
-            if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
-
-                //if the joystick is close to one of the cardinal directions, it is set to exactly the cardinal direction
-                faceDirection = snapToCardinal(rightJoystickDegrees,cardinalSnapping,0);
-            }
-            rightJoystickMagnitudePrevious = rightJoystickMagnitude;
-
-            //allows the the driver to indicate which direction the robot is currently looking so
-            //so that the controller and robot can be re-synced in the event of a bad initial
-            //position.
-            if (engine.gamepad1.right_stick_button) {
-                robot.resetRotation(faceDirection);
-            }
-
-            if (leftJoystickMagnitude == 0) {
-                double[] facePowers =  robot.getFacePowers(faceDirection,  TURN_POWER);
-                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
-            } else {
-                //drives the robot in the direction of the move joystick while facing the direction
-                //of the look joystick. if the move direction is almost aligned/perpendicular to the
-                //look joystick,
-                powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees,pairSnapping,faceDirection), drivePower, faceDirection);
-            }
-
-        }
-
-        robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
-
-        this.powers = powers;
-
-        if (childrenHaveFinished()) {
-            robot.collectionMotor.setPower(engine.gamepad2.right_trigger);
-        } else {
-            robot.collectionMotor.setPower(0);
-        }
-
-        boolean rb = engine.gamepad2.right_bumper;
-        if (rb && !rbPrev && childrenHaveFinished()) {
-            addParallelState(new ProgressRingBelt(robot));
-        }
-        rbPrev = rb;
-
-        boolean y2 = engine.gamepad2.y;
-        if (y2 && !yPrev && childrenHaveFinished()) {
-            launchState = new Launch(robot);
-            addParallelState(launchState);
-        }
-        yPrev = y2;
-
-        boolean x = engine.gamepad2.x;
-        if (x && !xPrev) {
-            wobbleGrabOpen = !wobbleGrabOpen;
-            if (wobbleGrabOpen) {
-                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
-            } else {
-                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.1 );
-            }
-        }
-        xPrev = x;
-
-        boolean b = engine.gamepad2.b;
-        if (b && !bPrev) {
-            wobbleArmUp = !wobbleArmUp;
-            if (wobbleArmUp) {
-                robot.wobbleArmMotor.setTargetPosition(550);
-            } else {
-                robot.wobbleArmMotor.setTargetPosition(0);
-            }
-        }
-        bPrev = b;
-
-        boolean emergencyLaunch = engine.gamepad2.a;
-        if (emergencyLaunch && !emergencyLaunchPrev) {
-            if (robot.launchMotor.getPower() == 0) {
-                robot.launchMotor.setPower(Robot.LAUNCH_POWER);
-            } else {
-                robot.launchMotor.setPower(0);
-            }
-        }
-        emergencyLaunchPrev = emergencyLaunch;
-
-        if (engine.gamepad2.dpad_up) {
-            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            robot.wobbleArmMotor.setPower(0.5);
-            manualArmHold = true;
-        } else if (engine.gamepad2.dpad_down) {
-            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            robot.wobbleArmMotor.setPower(-0.5);
-            manualArmHold = true;
-        } else if (manualArmHold) {
-            manualArmHold = false;
-            robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
-            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        }
-
-
-        boolean reverseBelt = engine.gamepad2.left_bumper;
-        if (reverseBelt && !reverseBeltPrev) {
-               beltPowerPrev = robot.ringBeltMotor.getPower();
-               robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
-        }
-
-        if (!reverseBelt && reverseBeltPrev) {
-            robot.ringBeltMotor.setPower(beltPowerPrev);
-        }
-
-        reverseBeltPrev = reverseBelt;
 
     }
 
     @Override
     public void telemetry() {
 
-       engine.telemetry.addData("childrenHaveFinished", childrenHaveFinished());
-       for (CyberarmState state : children) {
-           if (!state.getHasFinished()) {
-               engine.telemetry.addLine("" + state.getClass());
-           }
-       }
-
-       engine.telemetry.addData("wobble Arm Up", wobbleArmUp);
-
         engine.telemetry.addLine("Location");
         engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
         engine.telemetry.addData("Rotation ", robot.getRotation());
@@ -247,16 +40,4 @@ public class TeleOpState extends CyberarmState {
         return (float) (Math.floor(number/unit) * unit);
     }
 
-    private float snapToCardinal(float angle, float snapping, float offset) {
-        int o = (int) offset + 180;
-        o %= 90;
-        for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) {
-            if (angle >= cardinal-snapping && angle <= cardinal+snapping) {
-                angle = cardinal;
-            }
-        }
-        return angle;
-    }
-
-
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java

@@ -0,0 +1,33 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class UnstickRingBelt extends CyberarmState {
+
+    private Robot robot;
+    private int targetPos;
+    private double lastPower;
+
+    public UnstickRingBelt(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        lastPower = robot.ringBeltMotor.getPower();
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+        targetPos = robot.loopPos(currentPos - robot.beltReverseTicks);
+        robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
+
+    }
+
+    @Override
+    public void exec() {
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+        if (currentPos < targetPos) {
+            robot.ringBeltMotor.setPower(lastPower);
+            setHasFinished(true);
+        }
+    }
+
+}