End game Autononmous

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

@@ -1,10 +1,12 @@
 package org.timecrafters.UltimateGoal.Competition.Autonomous;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.cyberarm.engine.V2.CyberarmState;
 import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.Pause;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 import org.timecrafters.UltimateGoal.Competition.WobbleArm;
 import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
@@ -21,26 +23,19 @@ public class AutoEngine extends CyberarmEngine {
     private double launchPower;
     private long launchBrakeTime;
 
-    private float scoreAFaceAngle;
-    private double scoreATolerance;
-    private double scoreAPower;
-    private long scoreABrakeTime;
-
-    double parkY;
-    float parkFaceAngle;
-    double parkTolerance;
-    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);
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        robot.wobbleArmMotor.setTargetPosition(0);
+        robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
         // since we've preloaded three rings, the ring belt stage is set to account for this;
         robot.ringBeltStage = 3;
 
+        //Autonomous specific Variables
         float rotation = robot.stateConfiguration.variable("system", "startPos", "direction").value();
         double locationX = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "x").value());
         double locationY = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "y").value());
@@ -50,17 +45,6 @@ public class AutoEngine extends CyberarmEngine {
         launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
         launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();
 
-//        scoreAFaceAngle = robot.stateConfiguration.variable("auto","05_0","face").value();
-//        scoreATolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value());
-//        scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value();
-//        scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","brakeMS").value();
-//
-        parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","yPos").value());
-        parkFaceAngle = robot.stateConfiguration.variable("auto","13_0","face").value();
-        parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","tolPos").value());
-        parkPower = robot.stateConfiguration.variable("auto","13_0","power").value();
-        parkBrakeTime = robot.stateConfiguration.variable("auto","13_0","brakeMS").value();
-
         super.init();
     }
 
@@ -85,18 +69,18 @@ public class AutoEngine extends CyberarmEngine {
         //aligns to goal
         addState(new Face(robot, "auto", "04_1"));
 
-        //launch rings
+        //launch rings and drive to scoreing area. LaunchDriveControl allows makes the robot begin
+        //driving while the belt is resetting
         CyberarmState launchState = new Launch(robot, "auto", "04_2");
 
-        //drive to scoring area
         CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true);
         addState(new LaunchDriveControl(robot,launchState,driveState));
 
         //turn arm towards scoreing area.
-        ArrayList<CyberarmState> threadStates = new ArrayList<>();
+        ArrayList<CyberarmState> threadStates0 = new ArrayList<>();
-         threadStates.add(new Face(robot, "auto", "05_1"));
+         threadStates0.add(new Face(robot, "auto", "05_1"));
-        threadStates.add(new WobbleArm(robot, "auto", "05_2",false));
+        threadStates0.add(new WobbleArm(robot, "auto", "05_2",robot.wobbleDownPos));
-        addState(new ThreadStates(threadStates));
+        addState(new ThreadStates(threadStates0));
 
         //open the wobble grab arm
         addState(new WobbleGrab(robot, "auto", "06_0", true));
@@ -105,25 +89,32 @@ public class AutoEngine extends CyberarmEngine {
         addState(new DriveToCoordinates(robot, "auto","06_1"));
 
         addState(new DriveToCoordinates(robot, "auto", "07_0"));
-        addState(new FindWobbleGoal(robot, "auto", "08_0"));
+        addState(new Face(robot,"auto","07_1"));
 
-        //close grabber
+        addState(new FindWobbleGoal(robot, "auto", "08_0"));
-        addState(new WobbleGrab(robot, "auto", "09_0", false));
+        addState(new Pause(robot,"auto","09_0"));
 
         //drive to scoring area
-//        addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime));
+        ArrayList<CyberarmState> threadStates1 = new ArrayList<>();
-        addState(new DriveToCoordinates(robot, "auto", "10_0", true));
+//        threadStates1.add(new Face(robot, "auto", "09_0"));
+        threadStates1.add(new WobbleArm(robot, "auto", "09_1",robot.wobbleUpPos));
+        threadStates1.add(new DriveToCoordinates(robot, "auto", "10_0", true));
+        addState(new ThreadStates(threadStates1));
+
+
+
+        ArrayList<CyberarmState> threadStates2 = new ArrayList<>();
+        threadStates2.add(new Face(robot, "auto", "11_0"));
+        threadStates2.add(new WobbleArm(robot, "auto", "11_1",robot.wobbleDownPos));
+        addState(new ThreadStates(threadStates2));
 
         //release wobble goal 2
-        addState(new Face(robot, "auto", "11_0"));
         addState(new WobbleGrab(robot, "auto", "12_0", true));
 
         //drive to park
-        addState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
+        ArrayList<CyberarmState> threadStates3 = new ArrayList<>();
-//        ArrayList<CyberarmState> threadStatesB = new ArrayList<>();
+        threadStates3.add(new WobbleArm(robot, "auto", "12_2", 0));
-//        threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false));
+        threadStates3.add(new Park(robot,"auto","13_0"));
-//        threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true));
+        addState(new ThreadStates(threadStates3));
-//        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/FindWobbleGoal.java

@@ -16,6 +16,8 @@ public class FindWobbleGoal extends CyberarmState {
     private boolean cCheckPrev;
     private boolean ccCheckPrev;
     private float startRotation;
+    private boolean foundGoalRotation;
+    private float wobbleGoalRotation;
 
     public FindWobbleGoal(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -44,6 +46,7 @@ public class FindWobbleGoal extends CyberarmState {
         double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
 
         if (sensorValue > turnCheck) {
+
             float rotation = robot.getRelativeAngle(startRotation,robot.getRotation());
 
             boolean cCheck = rotation > range;
@@ -61,9 +64,15 @@ public class FindWobbleGoal extends CyberarmState {
 
         } else {
              if (sensorValue > driveCheck) {
-                 robot.setDrivePower(-power,-power,-power,-power);
+                 if (!foundGoalRotation) {
+                     foundGoalRotation = true;
+                     wobbleGoalRotation = robot.getRotation();
+                 }
+                 double[] powers = robot.getMecanumPowers(wobbleGoalRotation - 90, power*2 , wobbleGoalRotation);
+                 robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
              } else {
                  robot.setDrivePower(0,0,0,0);
+                 robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
                  setHasFinished(true);
              }
         }

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

@@ -0,0 +1,45 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class Park extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    double parkY;
+    float parkFaceAngle;
+    double parkTolerance;
+    double parkPower;
+    long parkBrakeTime;
+
+    public Park(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+        parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"yPos").value());
+        parkFaceAngle = robot.stateConfiguration.variable(groupName,actionName,"face").value();
+        parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto",actionName,"tolPos").value());
+        parkPower = robot.stateConfiguration.variable(groupName,actionName,"power").value();
+        parkBrakeTime = robot.stateConfiguration.variable(groupName,actionName,"brakeMS").value();
+    }
+
+    @Override
+    public void start() {
+        if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8))
+        addState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(childrenHaveFinished());
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java

@@ -1,164 +0,0 @@
-package org.timecrafters.UltimateGoal.Competition.Demo;
-
-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.Robot;
-
-public class Demo1 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;
-
-    private float launchAngleGoal;
-    private float launchAnglePower1;
-    private float launchAnglePower2;
-    private float launchAnglePower3;
-
-    public Demo1(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();
-
-    }
-
-    @Override
-    public void start() {
-        faceDirection = robot.getRotation();
-    }
-
-    @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.dpad_up;
-        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;
-
-        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);
-
-            //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.setLocalization(rightJoystickDegrees, robot.getLocationX(), robot.getLocationY());
-                faceDirection = rightJoystickDegrees;
-            }
-
-            //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;
-
-
-            if (leftJoystickMagnitude == 0) {
-                double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
-                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.addLine("Controler Directions");
-        engine.telemetry.addData("Right", rightJoystickDegrees);
-        engine.telemetry.addData("Left", leftJoystickDegrees);
-
-        engine.telemetry.addData("face", faceDirection);
-
-        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/Demo/Demo2.java

@@ -1,142 +0,0 @@
-package org.timecrafters.UltimateGoal.Competition.Demo;
-
-import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
-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 Demo2 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 Demo2(Robot robot) {
-        this.robot = robot;
-    }
-
-
-    @Override
-    public void init() {
-        robot.wobbleArmMotor.setTargetPosition(0);
-        robot.wobbleArmMotor.setPower(0.5);
-    }
-
-    @Override
-    public void start() {
-        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
-    }
-
-    @Override
-    public void exec() {
-
-        //Collector control
-        if (childrenHaveFinished()) {
-            robot.collectionMotor.setPower(engine.gamepad1.right_trigger);
-        } else {
-            robot.collectionMotor.setPower(0);
-        }
-
-        //belt progression control
-        boolean rb = engine.gamepad1.right_bumper;
-        if (rb && !rbPrev && childrenHaveFinished()) {
-            addParallelState(new ProgressRingBelt(robot));
-        }
-        rbPrev = rb;
-
-        //launch sequence control
-        boolean y2 = engine.gamepad1.y;
-        if (y2 && !yPrev && childrenHaveFinished()) {
-            addParallelState(new Launch(robot));
-        }
-        yPrev = y2;
-
-        //toggles wobble grabber open and closed
-        boolean x = engine.gamepad1.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.gamepad1.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.gamepad1.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.gamepad1.dpad_up) {
-            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            robot.wobbleArmMotor.setPower(0.5);
-            manualArmHold = true;
-        } else if (engine.gamepad1.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.gamepad1.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/Demo/DemoControl.java

@@ -15,8 +15,8 @@ public class DemoControl extends CyberarmState {
 
     @Override
     public void start() {
-        addParallelState(new Demo1(robot));
+//        addParallelState(new Demo1(robot));
-        addParallelState(new Demo2(robot));
+//        addParallelState(new Demo2(robot));
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java

@@ -1,11 +1,13 @@
 package org.timecrafters.UltimateGoal.Competition.Demo;
 
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 import org.timecrafters.UltimateGoal.Competition.TeleOp.TeleOpState;
 
+@Disabled
 @TeleOp (name = "Demo")
 public class DemoEngine extends CyberarmEngine {
 

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

@@ -31,14 +31,14 @@ public class Launch extends CyberarmState {
         try {
             if (robot.stateConfiguration.action(groupName, actionName).enabled) {
                 robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-                robot.ringBeltMotor.setPower(0.7);
+                robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
 
             } else {
                 setHasFinished(true);
             }
         } catch (NullPointerException e){
             robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            robot.ringBeltMotor.setPower(0.7);
+            robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
         }
 
     }
@@ -65,25 +65,25 @@ public class Launch extends CyberarmState {
             //the first receiving position.
 
             if (hasCycled) {
-                robot.ringBeltMotor.setPower(0);
                 robot.ringBeltStage = 0;
+                robot.ringBeltMotor.setTargetPosition(beltPos);
+                robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
                 if (!robot.initLauncher) {
                     robot.launchMotor.setPower(0);
                 }
 
                 setHasFinished(true);
-                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
             } else {
                 hasCycled = true;
-                reducePos = (int) (beltPos + (robot.reduceLaunchPos));
+                reducePos = beltPos + (robot.reduceLaunchPos);
             }
         }
         detectedPass = detectingPass;
 
         boolean reduceCondition = (hasCycled && beltPos > reducePos);
         if (reduceCondition && !reduceConditionPrev){
-            robot.ringBeltOn();
+            robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
 
             //the ring belt stage lets other states know that the robot has finished launching all three rings
             //and is now returning to loading position.

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

@@ -2,40 +2,37 @@ package org.timecrafters.UltimateGoal.Competition;
 
 import org.cyberarm.engine.V2.CyberarmState;
 
-public class CamServo extends CyberarmState {
+public class Pause extends CyberarmState {
 
     private Robot robot;
     private String groupName;
     private String actionName;
-    private boolean open;
     private long waitTime = -1;
+    private boolean enabled;
 
-    public CamServo(Robot robot, String groupName, String actionName, boolean open) {
+    public Pause(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
         this.actionName = actionName;
-        this.open = open;
     }
 
-    public CamServo(Robot robot, boolean armUp, long waitTime) {
+    public Pause(Robot robot, boolean open, long waitTime) {
         this.robot = robot;
-        this.open = armUp;
         this.waitTime = waitTime;
     }
 
     @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 (open) {
+        if (!enabled) {
-            robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+            setHasFinished(true);
-        } else {
-            robot.wobbleGrabServo.setPosition(0);
         }
     }
 
@@ -44,4 +41,10 @@ public class CamServo extends CyberarmState {
         setHasFinished(runTime() > waitTime);
     }
 
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("runTime", runTime());
+        engine.telemetry.addData("wait", waitTime);
+    }
+
 }

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

@@ -13,7 +13,7 @@ public class FindLimitSwitch extends CyberarmState {
 
     @Override
     public void start() {
-        robot.ringBeltOn();
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
     }
 
     @Override

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

@@ -19,7 +19,7 @@ public class ProgressRingBelt extends CyberarmState {
     private void prep(){
         robot.ringBeltMotor.setTargetPosition(targetPos);
         robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        robot.ringBeltMotor.setPower(0.7);
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
         robot.ringBeltStage += 1;
         robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
     }
@@ -28,7 +28,7 @@ public class ProgressRingBelt extends CyberarmState {
     public void start() {
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         if (robot.ringBeltStage < 2) {
-            targetPos = currentPos + Robot.RING_BELT_GAP;
+            targetPos = currentPos + robot.ringBeltGap;
             prep();
         } else if (robot.ringBeltStage == 2) {
             targetPos = currentPos + 240;

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

@@ -1,5 +1,11 @@
 package org.timecrafters.UltimateGoal.Competition;
 
+/*
+The robot object contains all the hardware and functions that are used in both teleOp and
+Autonomous. This includes drive functions, localization functions, shared constants, and a few
+general calculations and debugging tools.
+*/
+
 import android.os.Environment;
 import android.util.Log;
 
@@ -47,12 +53,18 @@ public class Robot {
         this.hardwareMap = hardwareMap;
     }
 
+    //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit
+    //variables saved on the phone, without having to re-download the whole program.
     public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
+
+    //We use the IMU to get reliable rotation and angular velocity information. Experimentation has
+    //demonstrated that the accelerometer and related integrations aren't as accurate.
     public BNO055IMU imu;
 
+    //The LEDs are used to provide driver feedback and for looking beautiful
     public RevBlinkinLedDriver ledDriver;
 
-    //drive system
+    //drive and dead-wheal hardware
     public DcMotor driveFrontLeft;
     public DcMotor driveBackLeft;
     public DcMotor driveFrontRight;
@@ -62,8 +74,9 @@ public class Robot {
     public DcMotor encoderRight;
     public DcMotor encoderBack;
 
-    //Steering Constants
+    //Motion Constants
-    static final double CUBIC_CORRECTION = 0.025;
+    static final double CUBIC_CORRECTION = 0.035;
+    static final double FACE_CUBIC_CORRECTION = 0.025;
     static final double LINEAR_CORRECTION = 0.055;
     static final double FACE_MIN_CORRECTION = 0.2;
     static final double FACE_LINEAR_CORRECTION = 0.025;
@@ -74,7 +87,7 @@ public class Robot {
     static final double FACE_MOMENTUM_CORRECTION = 1.06;
     static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1)/Math.log10(FACE_MOMENTUM_CORRECTION));
 
-    //Conversion Constants
+    //Unit Conversion Constants
     static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
     static final int COUNTS_PER_REVOLUTION = 8192;
     static final float mmPerInch = 25.4f;
@@ -83,16 +96,6 @@ public class Robot {
     static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4;
     static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6;
 
-    // Inches Forward of axis of rotation
-    static final float CAMERA_FORWARD_DISPLACEMENT  = 8f;
-    // Inches above Ground
-    static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
-    // Inches Left of axis of rotation
-    static final float CAMERA_LEFT_DISPLACEMENT = 4f;
-
-    static final double CAMERA_DISPLACEMENT_MAG = Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
-    static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
-
     //Robot Localization
     private static double locationX;
     private static double locationY;
@@ -104,6 +107,38 @@ public class Robot {
     private float rotationPrevious = 0;
     public float angularVelocity;
 
+    //vuforia navigation
+    private WebcamName webcam;
+    private VuforiaLocalizer vuforia;
+
+    // Inches Forward of axis of rotation
+    static final float CAMERA_FORWARD_DISPLACEMENT  = 8f;
+    // Inches above Ground
+    static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
+    // Inches Left of axis of rotation
+    static final float CAMERA_LEFT_DISPLACEMENT = 4f;
+
+    static final double CAMERA_DISPLACEMENT_MAG = Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
+    static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
+
+    public boolean trackableVisible;
+    private VuforiaTrackables targetsUltimateGoal;
+    private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
+    private OpenGLMatrix lastConfirmendLocation;
+
+    private long timeStartZeroVelocity = 0;
+    private long minCheckDurationMs = 500;
+    private int minCheckVelocity = 1;
+    private float vuforiaRotationCull;
+
+    //The servo mount for our camera allows us to look down for ideal TensorFlow and look up for
+    //ideal Vuforia Navigation
+    public Servo webCamServo;
+    public static final double CAM_SERVO_DOWN = 0.15;
+
+    //TensorFlow Object Detection
+    public TFObjectDetector tfObjectDetector;
+    private static final float MINIMUM_CONFIDENCE = 0.8f;
 
     //Launcher
     public DcMotor launchMotor;
@@ -114,57 +149,36 @@ public class Robot {
     public double launchPositionY;
     public float launchRotation;
     public int reduceLaunchPos;
-    public static final double LAUNCH_TOLERANCE_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
-    public static final double LAUNCH_TOLERANCE_FACE = 0.5;
 
     public boolean initLauncher;
 
     //Ring Intake
     public DcMotor collectionMotor;
-    public Rev2mDistanceSensor ringIntakeSensor;
-
-    public static final double RING_DETECT_DISTANCE = 100;
-    public static final double RING_DETECT_DELAY = 1000;
 
     //Ring Belt
     public DcMotor ringBeltMotor;
     public RevTouchSensor limitSwitch;
     public int ringBeltStage;
-    public static final int RING_BELT_LOOP_TICKS = 2544;
+    public int ringBeltGap = 700;
-    public static final int RING_BELT_GAP = 700;
+    public static final double RING_BELT_SLOW_POWER = 0.2;
-    public static final double RING_BELT_POWER = 0.2;
+    public static final double RING_BELT_NORMAL_POWER = 0.6;
     private int ringBeltPrev;
     public long beltMaxStopTime;
     public int beltReverseTicks;
     public int beltMaxStopTicks;
 
-    //Wobble Goal Arm
+    //Wobble Goal Arm & Grabber
-    public DcMotor wobbleArmMotor;
+    public DcMotor  wobbleArmMotor;
     public Servo  wobbleGrabServo;
-    public static final int WOBBLE_ARM_DOWN = -710;
+    public int wobbleDownPos;
-    public static final double WOBBLE_SERVO_MAX = 0.3;
+    public int wobbleUpPos;
+    public int wobbleDropPos;
+    public static final double WOBBLE_SERVO_OPEN = 0;
+    public static final double WOBBLE_SERVO_CLOSED = 1;
     public RevColorSensorV3 wobbleColorSensor;
     public double wobbleScoreX;
     public double wobbleScoreY;
-
+    public RevTouchSensor wobbleTouchSensor;
-    //vuforia navigation
-    private WebcamName webcam;
-    private VuforiaLocalizer vuforia;
-    public Servo webCamServo;
-    public static final double CAM_SERVO_DOWN = 0.15;
-
-    public boolean trackableVisible;
-    private VuforiaTrackables targetsUltimateGoal;
-    private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
-    private OpenGLMatrix lastConfirmendLocation;
-
-    private long timeStartZeroVelocity = 0;
-    private long minCheckDurationMs = 500;
-    private int minCheckVelocity = 1;
-
-    //TensorFlow Object Detection
-    public TFObjectDetector tfObjectDetector;
-    private static final float MINIMUM_CONFIDENCE = 0.8f;
 
     //Debugging
     public double totalV;
@@ -172,7 +186,6 @@ public class Robot {
     public double visionY;
     public double visionZ;
     public float rawAngle;
-//    private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
     private String TestingRecord = "x,y";
 
     public double forwardVector;
@@ -213,9 +226,15 @@ public class Robot {
         wobbleArmMotor.setTargetPosition(0);
         wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
+        wobbleUpPos = stateConfiguration.variable("system","arm", "up").value();
+        wobbleDownPos = stateConfiguration.variable("system","arm", "down").value();
+        wobbleDropPos = stateConfiguration.variable("system","arm", "drop").value();
+
         wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
 
         wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
+        wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
+
 
         //init ring belt
         collectionMotor = hardwareMap.dcMotor.get("collect");
@@ -228,6 +247,7 @@ public class Robot {
         beltMaxStopTime = stateConfiguration.variable("system","belt", "maxStopTime").value();
         beltMaxStopTicks = stateConfiguration.variable("system","belt", "maxStopTicks").value();
         beltReverseTicks = stateConfiguration.variable("system","belt", "reverseTicks").value();
+        ringBeltGap = stateConfiguration.variable("system","belt","gap").value();
 
         //init IMU
         imu  = hardwareMap.get(BNO055IMU.class, "imu");
@@ -259,8 +279,9 @@ public class Robot {
         webCamServo = hardwareMap.servo.get("look");
         webCamServo.setDirection(Servo.Direction.REVERSE );
 
-        minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value();
+        minCheckVelocity =stateConfiguration.variable("system", "camera", "minCheckV").value();
-        minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value();
+        vuforiaRotationCull = stateConfiguration.variable("system", "camera", "rCull").value();
+        minCheckDurationMs =stateConfiguration.variable("system", "camera", "minCheckMS").value();
 
         //Init Launch Motor
         DcMotor launcher = hardwareMap.dcMotor.get("launcher");
@@ -352,7 +373,7 @@ public class Robot {
         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
                 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
         TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
-        parameters.minResultConfidence = stateConfiguration.variable("system", "tensorFlow", "minConfidence").value();
+        parameters.minResultConfidence = stateConfiguration.variable("system", "camera", "minConfidence").value();
         tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
         tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
     }
@@ -413,22 +434,7 @@ public class Robot {
         Robot.rotation += rotationChange;
 
 
-//        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
+        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
-
-//
-//        if (totalV < minCheckVelocity) {
-//            long timeCurrent = System.currentTimeMillis();
-//
-//            if (timeStartZeroVelocity == 0) {
-//                timeStartZeroVelocity = timeCurrent;
-//            } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) {
-//                syncWithVuforia();
-//            }
-//
-//        } else {
-//            timeStartZeroVelocity = 0;
-//        }
-
 
         if (Robot.rotation > 180) {
             Robot.rotation -= 360;
@@ -439,6 +445,22 @@ public class Robot {
 
     }
 
+    public void syncIfStationary() {
+        if (totalV < minCheckVelocity) {
+            long timeCurrent = System.currentTimeMillis();
+
+            if (timeStartZeroVelocity == 0) {
+                timeStartZeroVelocity = timeCurrent;
+            } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs ) {
+                syncWithVuforia();
+            }
+
+        } else {
+            timeStartZeroVelocity = 0;
+        }
+
+    }
+
     public void syncWithVuforia() {
         trackableVisible = false;
         for (VuforiaTrackable trackable : trackables) {
@@ -454,24 +476,28 @@ public class Robot {
 
                 //For our tournament, it makes sense to make zero degrees towards the goal.
                 //Orientation is inverted to have clockwise be positive.
-                Orientation rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
+                Orientation orientation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
-                Robot.rotation = 90-rotation.thirdAngle;
+                float vuforiaRotation = 90-orientation.thirdAngle;
 
-                if (Robot.rotation > 180) {
+                if (vuforiaRotation > 180) {
-                    Robot.rotation -= -180;
+                    vuforiaRotation -= -180;
                 }
 
-                VectorF translation = lastConfirmendLocation.getTranslation();
+                if (Math.abs(rotation - vuforiaRotation) < vuforiaRotationCull) {
-                double camX = -translation.get(1) / mmPerInch;
+                    rotation = vuforiaRotation;
-                double camY = translation.get(0) / mmPerInch;
 
-                double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                    VectorF translation = lastConfirmendLocation.getTranslation();
-                double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                    double camX = -translation.get(1) / mmPerInch;
+                    double camY = translation.get(0) / mmPerInch;
 
-                locationX = inchesToTicks(camX - displaceX);
+                    double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
-                locationY = inchesToTicks(camY - displaceY);
+                    double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
 
-                break;
+                    locationX = inchesToTicks(camX - displaceX);
+                    locationY = inchesToTicks(camY - displaceY);
+
+                    break;
+                }
             }
         }
     }
@@ -570,22 +596,21 @@ public class Robot {
                 Math.pow(CUBIC_CORRECTION * relativeRotation, 3) +
                         LINEAR_CORRECTION * relativeRotation;
 
+        if (relativeRotation != 0) {
+            double momentumRelative =  angularVelocity * (relativeRotation / Math.abs(relativeRotation));
+            double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+            double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
+            //reduces concern for  momentum when the angle is far away from target
+            turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) * (1  - momentumCorrection)) / 180 );
+//            turnCorrection *= momentumCorrection;
+        }
+
         double powerForwardRight = scalar * (q + turnCorrection);
         double powerForwardLeft = scalar * (p - turnCorrection);
         double powerBackRight = scalar * (p + turnCorrection);
         double powerBackLeft = scalar * (q - turnCorrection);
 
 
-        if (relativeRotation != 0) {
-            double momentumRelative =  angularVelocity * (relativeRotation / Math.abs(relativeRotation));
-            double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
-            double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
-            powerForwardRight *= momentumCorrection;
-            powerForwardLeft *= momentumCorrection;
-            powerBackRight *= momentumCorrection;
-            powerBackLeft *= momentumCorrection;
-        }
-
         // The "extreme" is the power value that is furthest from zero. When this values exceed the
         // -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the
         // workable range without altering the final motion vector.
@@ -610,7 +635,7 @@ public class Robot {
     public double[] getFacePowers(float direction, double power) {
         angularVelocity = imu.getAngularVelocity().xRotationRate;
         double relativeAngle = getRelativeAngle(direction, Robot.rotation);
-        double scaler = Math.pow(CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle;
+        double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle;
 
         if (relativeAngle > 0.5) {
             scaler += FACE_MIN_CORRECTION;
@@ -633,10 +658,6 @@ public class Robot {
         return powers;
     }
 
-    public void ringBeltOn() {
-        ringBeltMotor.setPower(RING_BELT_POWER);
-    }
-
     public boolean beltIsStuck() {
         int ringBeltPos = ringBeltMotor.getCurrentPosition();
         boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks);

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

@@ -30,6 +30,7 @@ public class Player1 extends CyberarmState {
     private FindWobbleGoal findWobbleGoal;
     private boolean runNextFindWobble;
     private boolean findWobbleInputPrev;
+    private boolean aPrev;
 
     //Drive to launch control
     private DriveToCoordinates driveToLaunch;
@@ -84,20 +85,43 @@ public class Player1 extends CyberarmState {
         }
         lbPrev = lb;
 
+        if (engine.gamepad1.guide) {
+            robot.syncIfStationary();
+        }
+
         runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
 
         boolean findWobbleInput = engine.gamepad1.x;
         if (findWobbleInput) {
-            if (runNextFindWobble && !findWobbleInputPrev) {
+            //if the claw is open, run FindWobbleGoal
-                findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
+            if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
-                addParallelState(findWobbleGoal);
+
+                faceDirection = robot.getRotation();
+
+                if (runNextFindWobble && !findWobbleInputPrev) {
+                    findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
+                    addParallelState(findWobbleGoal);
+                }
+                //if the claw is closed, open the claw.
+            } else if (!findWobbleInputPrev) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
             }
-            faceDirection = robot.getRotation();
+            //if the button is released cancel the search
         } else if (!runNextFindWobble) {
             findWobbleGoal.setHasFinished(true);
         }
         findWobbleInputPrev = findWobbleInput;
 
+        //toggles wobble grabber open and closed
+        boolean a = engine.gamepad1.a;
+        if (a && !aPrev) {
+            if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+            } else {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
+            }
+        }
+        aPrev = a;
 
         runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished());
 
@@ -168,7 +192,7 @@ public class Player1 extends CyberarmState {
             robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
         }
 
-
+        //LED feedback control
         double ringBeltPower = robot.ringBeltMotor.getPower();
         if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - robot.ringBeltMotor.getCurrentPosition()) > 10) {
             robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );

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

@@ -23,6 +23,8 @@ public class Player2 extends CyberarmState {
     private boolean lbPrev;
     private boolean manualArmHold;
 
+
+
     private boolean launchInput = false;
 
     public Player2(Robot robot) {
@@ -45,14 +47,13 @@ public class Player2 extends CyberarmState {
     public void exec() {
 
         //Collector control
-        if (childrenHaveFinished()) {
+
-            double rt = engine.gamepad2.right_trigger;
+        double rt = engine.gamepad2.right_trigger;
-            double lt = engine.gamepad2.left_trigger;
+        double lt = engine.gamepad2.left_trigger;
-            if (rt >= lt) {
+        if (rt < lt) {
-                robot.collectionMotor.setPower(rt);
+            robot.collectionMotor.setPower(-lt);
-            } else {
+        } else if (childrenHaveFinished()) {
-                robot.collectionMotor.setPower(-lt);
+            robot.collectionMotor.setPower(rt);
-            }
         } else {
             robot.collectionMotor.setPower(0);
         }
@@ -71,29 +72,48 @@ public class Player2 extends CyberarmState {
         }
         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;
+//        boolean b = engine.gamepad2.b;
-        if (b && !bPrev) {
+//        if (b && !bPrev) {
-            wobbleArmUp = !wobbleArmUp;
+//            wobbleArmUp = !wobbleArmUp;
-            if (wobbleArmUp) {
+//            if (wobbleArmUp) {
-                robot.wobbleArmMotor.setTargetPosition(550);
+//                robot.wobbleArmMotor.setTargetPosition(550);
+//            } else {
+//                robot.wobbleArmMotor.setTargetPosition(0);
+//            }
+//        }
+//        bPrev = b;
+
+        //manually control the wobble arm for when it's initialized in an unexpected position.
+        double leftStickY = engine.gamepad2.left_stick_y;
+
+        if (engine.gamepad2.dpad_right) {
+            if (!robot.wobbleTouchSensor.isPressed()) {
+                setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.wobbleArmMotor.setPower(-0.2);
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
             } else {
+                setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
                 robot.wobbleArmMotor.setTargetPosition(0);
+                setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+        } else if (leftStickY != 0 ) {
+            setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.wobbleArmMotor.setPower(0.15 * leftStickY);
+            robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
+        } else {
+            setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
+            robot.wobbleArmMotor.setPower(0.5);
+            if (engine.gamepad2.dpad_up) {
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleUpPos);
+            } else if (engine.gamepad2.dpad_down) {
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleDownPos);
+            } else if (engine.gamepad2.dpad_left) {
+                robot.wobbleArmMotor.setTargetPosition(robot.wobbleDropPos );
             }
         }
-        bPrev = b;
 
         //manually toggle the launch wheel for emergencies
         boolean a = engine.gamepad2.a;
@@ -106,20 +126,8 @@ public class Player2 extends CyberarmState {
         }
         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;
@@ -127,7 +135,7 @@ public class Player2 extends CyberarmState {
             runModePrev = robot.ringBeltMotor.getMode();
             beltPowerPrev = robot.ringBeltMotor.getPower();
             robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-            robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
+            robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
         }
 
         if (!lb && lbPrev) {
@@ -138,13 +146,21 @@ public class Player2 extends CyberarmState {
         lbPrev = lb;
     }
 
+    private void setArmMode(DcMotor.RunMode runMode) {
+        if (robot.wobbleArmMotor.getMode() != runMode) {
+             robot.wobbleArmMotor.setMode(runMode);
+        }
+    }
+
     @Override
     public void telemetry() {
-        engine.telemetry.addLine("belt");
+//        engine.telemetry.addLine("belt");
-        engine.telemetry.addData("power", robot.ringBeltMotor.getPower());
+//        engine.telemetry.addData("power", robot.ringBeltMotor.getPower());
-        engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
+//        engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
-        engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
+//        engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
 
+        engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed());
+        engine.telemetry.addData("  Sensor value", robot.wobbleTouchSensor.getValue());
         engine.telemetry.addData("Player 2 children", childrenHaveFinished());
         for (CyberarmState state : children) {
             if (!state.getHasFinished()) {

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

@@ -14,7 +14,7 @@ public class TeleOpEngine extends CyberarmEngine {
     public void init() {
         robot = new Robot(hardwareMap);
         robot.initHardware();
-        robot.wobbleGrabServo.setPosition(0);
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
         robot.webCamServo.setPosition(0);
         super.init();
     }

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

@@ -1,6 +1,5 @@
 package org.timecrafters.UltimateGoal.Competition;
 
-import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -22,7 +21,7 @@ public class UnstickRingBelt extends CyberarmState {
         lastRunMode = robot.ringBeltMotor.getMode();
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         targetPos = currentPos - robot.beltReverseTicks;
-        robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
+        robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
     }
 
     @Override

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

@@ -7,19 +7,20 @@ public class WobbleArm extends CyberarmState {
     private Robot robot;
     private String groupName;
     private String actionName;
-    private boolean armUp;
+    private int suggestedPos;
+    private int targetPos = -1;
     private long waitTime = -1;
 
-    public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) {
+    public WobbleArm(Robot robot, String groupName, String actionName, int targetPos) {
         this.robot = robot;
         this.groupName  = groupName;
         this.actionName = actionName;
-        this.armUp = armUp;
+        this.suggestedPos = targetPos;
     }
 
-    public WobbleArm(Robot robot, boolean armUp, long waitTime) {
+    public WobbleArm(Robot robot, int targetPos, long waitTime) {
         this.robot = robot;
-        this.armUp = armUp;
+        this.targetPos = targetPos;
         this.waitTime = waitTime;
     }
 
@@ -27,17 +28,17 @@ public class WobbleArm extends CyberarmState {
     public void init() {
         if (waitTime == -1) {
             waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
+            targetPos = robot.stateConfiguration.variable(groupName,actionName,"armPos").value();
+            if (targetPos == -1) {
+                targetPos = suggestedPos;
+            }
         }
     }
 
     @Override
     public void start() {
-        robot.wobbleArmMotor.setPower(0.3);
+        robot.wobbleArmMotor.setPower(0.5);
-        if (armUp) {
+        robot.wobbleArmMotor.setTargetPosition(targetPos);
-            robot.wobbleArmMotor.setTargetPosition(0);
-        } else {
-            robot.wobbleArmMotor.setTargetPosition(Robot.WOBBLE_ARM_DOWN);
-        }
     }
 
     @Override
@@ -48,5 +49,7 @@ public class WobbleArm extends CyberarmState {
     @Override
     public void telemetry() {
         engine.telemetry.addData("runTime", runTime());
+        engine.telemetry.addData("target", targetPos);
+        engine.telemetry.addData("wait", waitTime);
     }
 }

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

@@ -36,9 +36,9 @@ public class WobbleGrab extends CyberarmState {
     public void start() {
         if (enabled) {
             if (open) {
-                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
             } else {
-                robot.wobbleGrabServo.setPosition(0);
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
             }
         } else {
             setHasFinished(true);
@@ -50,4 +50,10 @@ public class WobbleGrab extends CyberarmState {
         setHasFinished(runTime() > waitTime);
     }
 
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("runTime", runTime());
+        engine.telemetry.addData("wait", waitTime);
+    }
+
 }

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

@@ -20,12 +20,11 @@ public class ServoPosTest extends CyberarmState {
     @Override
     public void init() {
         servo = engine.hardwareMap.servo.get("look");
-        servo.setDirection(Servo.Direction.REVERSE );
     }
 
     @Override
     public void exec() {
-        servoPos = engine.gamepad1.right_stick_y * 0.2;
+        servoPos = engine.gamepad1.right_stick_y;
         servo.setPosition(servoPos);
     }
 

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

@@ -13,7 +13,7 @@ public class TestingEngine extends CyberarmEngine {
 
     @Override
     public void setup() {
-        addState(new LEDTest());
+        addState(new ServoPosTest());
     }
 
 }