Emergency features & autonomous touches

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

@@ -26,16 +26,20 @@ public class AutoEngine extends CyberarmEngine {
     private double scoreAPower;
     private long scoreABrakeTime;
 
-    float scoreBFaceAngle;
+    double parkY;
-    double scoreBTolerance;
+    float parkFaceAngle;
-    double scoreBPower;
+    double parkTolerance;
-    long scoreBBrakeTime;
+    double parkPower;
+    long parkBrakeTime;
 
     @Override
     public void init() {
         robot = new Robot(hardwareMap);
         robot.initHardware();
         robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN);
+        robot.wobbleGrabServo.setPosition(0.1  * Robot.WOBBLE_SERVO_MAX);
+        // since we've preloaded three rings, the ring belt stage is set to account for this;
+        robot.ringBeltStage = 3;
 
         launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
         launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
@@ -46,10 +50,11 @@ public class AutoEngine extends CyberarmEngine {
 //        scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value();
 //        scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","brakeMS").value();
 //
-//        scoreBFaceAngle = robot.stateConfiguration.variable("auto","10_0","face").value();
+        parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","yPos").value());
-//        scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","10_0","tolPos").value());
+        parkFaceAngle = robot.stateConfiguration.variable("auto","13_0","face").value();
-//        scoreBPower = robot.stateConfiguration.variable("auto","10_0","power").value();
+        parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","tolPos").value());
-//        scoreBBrakeTime = robot.stateConfiguration.variable("auto","10_0","brakeMS").value();
+        parkPower = robot.stateConfiguration.variable("auto","13_0","power").value();
+        parkBrakeTime = robot.stateConfiguration.variable("auto","13_0","brakeMS").value();
 
         super.init();
     }
@@ -72,15 +77,15 @@ public class AutoEngine extends CyberarmEngine {
         //drive to launch position
         addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime));
 
-        //
+        //aligns to goal
         addState(new Face(robot, "auto", "04_1"));
 
         //launch rings
-        addState(new Launch(robot, "auto", "04_2"));
+        CyberarmState launchState = new Launch(robot, "auto", "04_2");
 
         //drive to scoring area
-        addState(new DriveToCoordinates(robot, "auto", "05_0", true));
+        CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true);
-//        addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime));
+        addState(new LaunchDriveControl(robot,launchState,driveState));
 
         //turn arm towards scoreing area.
         ArrayList<CyberarmState> threadStates = new ArrayList<>();
@@ -95,7 +100,7 @@ public class AutoEngine extends CyberarmEngine {
         addState(new DriveToCoordinates(robot, "auto","06_1"));
 
         addState(new DriveToCoordinates(robot, "auto", "07_0"));
-        addState(new DriveWithColorSensor(robot, "auto", "08_0"));
+        addState(new FindWobbleGoal(robot, "auto", "08_0"));
 
         //close grabber
         addState(new WobbleGrab(robot, "auto", "09_0", false));
@@ -109,10 +114,11 @@ public class AutoEngine extends CyberarmEngine {
         addState(new WobbleGrab(robot, "auto", "12_0", true));
 
         //drive to park
-        ArrayList<CyberarmState> threadStatesB = new ArrayList<>();
+        addState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
-        threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false));
+//        ArrayList<CyberarmState> threadStatesB = new ArrayList<>();
-        threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true));
+//        threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false));
-        threadStatesB.add(new DriveToCoordinates(robot, "auto", "13_0"));
+//        threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true));
-        addState(new ThreadStates(threadStatesB));
+//        threadStatesB.add(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
+//        addState(new ThreadStates(threadStatesB));
     }
 }

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

@@ -45,6 +45,8 @@ public class DriveToCoordinates extends CyberarmState {
     @Override
     public void init() {
         if (!groupName.equals("manual")) {
+            xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
+            yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
             power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
             tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
             brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
@@ -62,12 +64,15 @@ public class DriveToCoordinates extends CyberarmState {
         if (!groupName.equals("manual")) {
             setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
 
-            if (!scoringArea) {
+            //used to navigate towards the randomly generated scoreing area. the original target
-                xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
+            //becomes an offset of the scoring area position.
-                yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
+            if (scoringArea) {
-            } else {
+                xTarget += robot.wobbleScoreX;
-                xTarget = robot.wobbleScoreX;
+                yTarget += robot.wobbleScoreY;
-                yTarget = robot.wobbleScoreY;
+            }
+
+            if (faceAngle == 360) {
+                faceAngle = robot.getRelativeAngle(180, robot.getAngleToPosition(xTarget,yTarget));
             }
         }
 

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

@@ -58,6 +58,12 @@ public class Face extends CyberarmState {
                 double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
                 faceAngle = robot.getAngleToPosition(x, y);
             }
+
+            if (faceAngle == 360) {
+                double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value());
+                double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
+                faceAngle = robot.getRelativeAngle(180,robot.getAngleToPosition(x, y));
+            }
         }
     }
 

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

@@ -0,0 +1,76 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class FindWobbleGoal extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private double power;
+    private double turnCheck;
+    private double driveCheck;
+    private double range;
+    private boolean cCheckPrev;
+    private boolean ccCheckPrev;
+
+    public FindWobbleGoal(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+        power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
+        turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value();
+        driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value();
+        range = robot.stateConfiguration.variable(groupName,actionName,"r").value();
+    }
+
+    @Override
+    public void start() {
+        setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
+        robot.setDrivePower(power,-power,power,-power);
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+        double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
+
+        if (sensorValue > turnCheck) {
+            boolean cCheck = robot.getRotation() > range;
+            boolean ccCheck = robot.getRotation() < -range;
+
+            if (cCheck && !cCheckPrev) {
+                robot.setDrivePower(-power, power, -power, power);
+            }
+
+            if (ccCheck && !ccCheckPrev) {
+                robot.setDrivePower(power, -power, power, -power);
+            }
+            cCheckPrev = cCheck;
+            ccCheckPrev = ccCheck;
+
+        } else {
+             if (sensorValue > driveCheck) {
+                 robot.setDrivePower(-power,-power,-power,-power);
+             } else {
+                 robot.setDrivePower(0,0,0,0);
+                 setHasFinished(true);
+             }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("sensor", robot.wobbleColorSensor.getDistance(DistanceUnit.MM));
+        engine.telemetry.addData("red", robot.wobbleColorSensor.red());
+        engine.telemetry.addData("green", robot.wobbleColorSensor.green());
+        engine.telemetry.addData("blue", robot.wobbleColorSensor.blue());
+    }
+}
+

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

@@ -0,0 +1,41 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class LaunchDriveControl extends CyberarmState {
+
+    private Robot robot;
+    private CyberarmState launchState;
+    private CyberarmState driveState;
+    private boolean checkConditionPrev;
+
+    public LaunchDriveControl(Robot robot, CyberarmState launchState, CyberarmState driveState) {
+        this.robot = robot;
+        this.launchState = launchState;
+        this.driveState = driveState;
+    }
+
+    @Override
+    public void start() {
+        addParallelState(launchState);
+    }
+
+    @Override
+    public void exec() {
+        boolean checkCondition = (robot.ringBeltStage > 3);
+        if (checkCondition && !checkConditionPrev) {
+            addParallelState(driveState);
+        }
+        checkConditionPrev = checkCondition;
+
+        setHasFinished(childrenHaveFinished());
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
+        engine.telemetry.addData("check prev", checkConditionPrev);
+        engine.telemetry.addData("drive  prev", checkConditionPrev);
+    }
+}

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

@@ -9,6 +9,8 @@ public class Launch extends CyberarmState {
     private String actionName;
     boolean hasCycled;
     boolean detectedPass;
+    private boolean reduceConditionPrev;
+    private double reducePos;
 
     public Launch(Robot robot) {
         this.robot = robot;
@@ -24,12 +26,12 @@ public class Launch extends CyberarmState {
     public void start() {
         try {
             if (robot.stateConfiguration.action(groupName, actionName).enabled) {
-                robot.ringBeltOn();
+                robot.ringBeltMotor.setPower(0.5);
             } else {
                 setHasFinished(true);
             }
         } catch (NullPointerException e){
-            robot.ringBeltOn();
+            robot.ringBeltMotor.setPower(0.5);
         }
     }
 
@@ -37,9 +39,12 @@ public class Launch extends CyberarmState {
     public void exec() {
         //detect when limit switch is initially triggered
         boolean detectingPass = robot.limitSwitch.isPressed();
+        int beltPos = robot.getBeltPos();
+
         if (detectingPass && !detectedPass) {
             //finish once the ring belt has cycled all the way through and then returned to
             //the first receiving position.
+
             if (hasCycled) {
                 robot.ringBeltMotor.setPower(0);
                 robot.ringBeltStage = 0;
@@ -51,9 +56,24 @@ public class Launch extends CyberarmState {
                 setHasFinished(true);
             } else {
                 hasCycled = true;
+                reducePos = robot.loopPos((int) (beltPos + (1.5 * Robot.RING_BELT_GAP)));
             }
         }
         detectedPass = detectingPass;
 
+        boolean reduceCondition = (hasCycled && beltPos > reducePos && beltPos < reducePos + Robot.RING_BELT_GAP);
+        if (reduceCondition && !reduceConditionPrev){
+            robot.ringBeltOn();
+            //the ring belt stage lets other states know that the robot has finished launching all three rings
+            //and is now returning to loading position.
+
+            robot.ringBeltStage += 1;
+        }
+        reduceConditionPrev = reduceCondition;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("hasCycled", hasCycled);
     }
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java

@@ -26,5 +26,10 @@ public class LoadRings extends CyberarmState {
             addParallelState(ringBeltState);
 
         }
+
+        if (robot.ringBeltStage > 2) {
+            robot.launchMotor.setPower(0);
+            setHasFinished(true);
+        }
     }
 }

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

@@ -98,7 +98,7 @@ public class Robot {
 
     //Launcher
     public DcMotor launchMotor;
-    public static final double LAUNCH_POWER = 0.7;
+    public static final double LAUNCH_POWER = 0.715;
 
     private static final long LAUNCH_ACCEL_TIME = 500;
     public double launchPositionX;
@@ -364,8 +364,8 @@ public class Robot {
         rotationPrevious = imuAngle;
 
         //The forward Vector has the luxury of having an odometer on both sides of the robot.
-        //This allows us to eliminate the unwanted influence of turning the robot by averaging
+        //This allows us to reduce the unwanted influence of turning the robot by averaging
-        //the two.
+        //the two. unfortunatly we the current positioning of the odometers
 
         //Since there isn't a second wheel to remove the influence of robot rotation, we have to
         //instead do this by approximating the number of ticks that were removed due to rotation
@@ -474,6 +474,10 @@ public class Robot {
         return locationY;
     }
 
+    public void resetRotation(float rotation) {
+        this.rotation = rotation;
+    }
+
     //Manually set the position of the robot on the field.
     public void setCurrentPosition(float rotation, double x, double y) {
         this.rotation = rotation;
@@ -541,27 +545,20 @@ public class Robot {
         double p = y + x;
         double q = y - x;
 
+
+
         //calculating correction needed to steer the robot towards the degreesDirectionFace
-        float relativeRotation =  getRelativeAngle(degreesDirectionFace, rotation);
+        float relativeRotation =
-        double turnCorrection = Math.pow(LARGE_CORRECTION * relativeRotation, 3) + FINE_CORRECTION * relativeRotation;
+                getRelativeAngle(degreesDirectionFace, rotation);
+        double turnCorrection =
+                Math.pow(LARGE_CORRECTION * relativeRotation, 3) +
+                        FINE_CORRECTION * relativeRotation;
 
         double powerForwardRight = scalar * (q + turnCorrection);
         double powerForwardLeft = scalar * (p - turnCorrection);
         double powerBackRight = scalar * (p + turnCorrection);
         double powerBackLeft = scalar * (q - turnCorrection);
 
-//        //the turnCorrection often results in powers with magnitudes significantly larger than the
-//        //scalar. The scaleRatio mitigates this without altering the quality of the motion by making
-//        //it so that the average of the four magnitudes is equal to the scalar magnitude.
-//        double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) +
-//                Math.abs(powerBackRight) + Math.abs(powerBackLeft);
-//        double scaleRatio = (4 * Math.abs(scalar))/powerSum;
-//
-//        powerForwardRight *= scaleRatio;
-//        powerForwardLeft *= scaleRatio;
-//        powerBackRight *= scaleRatio;
-//        powerBackLeft *= scaleRatio;
-
 
         if (relativeRotation != 0) {
             double momentumRelative =  angularVelocity * (relativeRotation / Math.abs(relativeRotation));
@@ -588,15 +585,6 @@ public class Robot {
             powerBackLeft = powerBackLeft/extreme;
         }
 
-//        double powerControlThreshold = 0.6 * ;
-//
-//        if (Math.min(extreme, 1) > powerControlThreshold) {
-//            powerForwardLeft *= powerControlThreshold;
-//            powerForwardRight *= powerControlThreshold;
-//            powerBackLeft *= powerControlThreshold;
-//            powerBackRight *= powerControlThreshold;
-//        }
-
         double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
 
         return powers;

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

@@ -1,5 +1,7 @@
 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;
@@ -27,18 +29,23 @@ public class TeleOpState extends CyberarmState {
     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 xPrev;
+    private boolean rbPrev;
     private boolean yPrev;
-    private boolean aPrev;
+    private boolean xPrev;
     private boolean bPrev;
     private boolean lbPrev;
-    private boolean wobbleArmUp = true;
+    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;
 
@@ -47,11 +54,17 @@ public class TeleOpState extends CyberarmState {
         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);
     }
 
     @Override
@@ -59,25 +72,6 @@ public class TeleOpState extends CyberarmState {
         robot.updateLocation();
         robot.record(""+robot.ticksToInches(robot.getLocationX())+", "+robot.ticksToInches(robot.getLocationY()));
 
-        double leftJoystickX = engine.gamepad1.left_stick_x;
-        double leftJoystickY = engine.gamepad1.left_stick_y;
-
-        leftJoystickDegrees = (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 = (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;
-
         boolean lb = engine.gamepad1.left_stick_button;
         if (lb && !lbPrev) {
             if (drivePower == 1) {
@@ -91,37 +85,58 @@ public class TeleOpState extends CyberarmState {
         double[] powers = {0,0,0,0};
 
         boolean y = engine.gamepad1.y;
+
         if (y) {
 
-            //Launch Sequence
+            //Drive to Launch Pos
-
             double distanceToTarget = Math.hypot(robot.launchPositionX - robot.getLocationX(), robot.launchPositionY - robot.getLocationY());
-            if (distanceToTarget > Robot.LAUNCH_TOLERANCE_POS) {
+            if (distanceToTarget > LAUNCH_TOLERANCE_POS) {
                 powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.launchPositionX, robot.launchPositionY), drivePower, robot.launchRotation);
 
-            } else if (robot.getRelativeAngle(robot.launchRotation, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) {
+            } else if (Math.abs(robot.getRelativeAngle(robot.launchRotation,robot.getRotation())) > LAUNCH_TOLERANCE_FACE) {
-
-                launchState = new Launch(robot);
-                addParallelState(launchState);
-
-            } else {
-
                 double[] facePowers =  robot.getFacePowers(robot.launchRotation, TURN_POWER);
                 powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
+            } else {
 
             }
 
-        }
+        } else {
-
-        if (!y || ( launchState != null && launchState.getHasFinished())) {
-
             //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);
             }
 
@@ -137,11 +152,11 @@ public class TeleOpState extends CyberarmState {
             robot.collectionMotor.setPower(0);
         }
 
-        boolean x = engine.gamepad2.x;
+        boolean rb = engine.gamepad2.right_bumper;
-        if (x && !xPrev && childrenHaveFinished()) {
+        if (rb && !rbPrev && childrenHaveFinished()) {
             addParallelState(new ProgressRingBelt(robot));
         }
-        xPrev = x;
+        rbPrev = rb;
 
         boolean y2 = engine.gamepad2.y;
         if (y2 && !yPrev && childrenHaveFinished()) {
@@ -150,24 +165,64 @@ public class TeleOpState extends CyberarmState {
         }
         yPrev = y2;
 
-        boolean a = engine.gamepad2.a;
+        boolean x = engine.gamepad2.x;
-        if (a && !aPrev && childrenHaveFinished()) {
+        if (x && !xPrev) {
             wobbleGrabOpen = !wobbleGrabOpen;
-            addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100));
+            if (wobbleGrabOpen) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+            } else {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.1 );
             }
-        aPrev = a;
+        }
+        xPrev = x;
 
         boolean b = engine.gamepad2.b;
-        if (b && !bPrev && childrenHaveFinished()) {
+        if (b && !bPrev) {
             wobbleArmUp = !wobbleArmUp;
-            addParallelState(new WobbleArm(robot, wobbleArmUp, 100));
+            if (wobbleArmUp) {
+                robot.wobbleArmMotor.setTargetPosition(550);
+            } else {
+                robot.wobbleArmMotor.setTargetPosition(0);
+            }
         }
         bPrev = b;
 
-
+        boolean emergencyLaunch = engine.gamepad2.a;
-        if (engine.gamepad2.right_bumper && engine.gamepad2.left_bumper) {
+        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;
 
     }
 
@@ -181,9 +236,7 @@ public class TeleOpState extends CyberarmState {
            }
        }
 
-       engine.telemetry.addData("Vision Z", robot.visionZ);
+       engine.telemetry.addData("wobble Arm Up", wobbleArmUp);
-
-       engine.telemetry.addData("belt stage", robot.ringBeltStage);
 
         engine.telemetry.addLine("Location");
         engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");

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

@@ -9,6 +9,7 @@ public class WobbleGrab extends CyberarmState {
     private String actionName;
     private boolean open;
     private long waitTime = -1;
+    private boolean enabled;
 
     public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) {
         this.robot = robot;
@@ -26,13 +27,14 @@ public class WobbleGrab extends CyberarmState {
     @Override
     public void init() {
         if (waitTime == -1) {
+            enabled = robot.stateConfiguration.action(groupName, actionName).enabled;
             waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
         }
     }
 
     @Override
     public void start() {
-        if (robot.stateConfiguration.action(groupName, actionName).enabled) {
+        if (enabled) {
             if (open) {
                 robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
             } else {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java

@@ -3,22 +3,23 @@ package org.timecrafters.UltimateGoal.HardwareTesting;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 
 @TeleOp (name = "Hardware test", group = "test")
 public class TestingEngine extends CyberarmEngine {
 
-//    private Robot robot;
+    private Robot robot;
-//    @Override
+    @Override
-//    public void init() {
+    public void init() {
-//        robot = new Robot(hardwareMap);
+        robot = new Robot(hardwareMap);
-//        robot.initHardware();
+        robot.initHardware();
-//        super.init();
+        super.init();
-//    }
+    }
 
     @Override
     public void setup() {
-        addState(new ServoPosTest());
+        addState(new FindWobbleGoal(robot, "auto", "08_0"));
     }
 
 //    @Override