Full Autonomous First pass

TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java

@@ -72,7 +72,11 @@ public abstract class CyberarmState implements Runnable {
     Log.i(TAG, "Adding " + state.getClass() + " to " + this.getClass());
     children.add(state);
 
-    if (isRunning()) { state.init(); engine.runState(state); }
+    if (isRunning()) {
+      state.init();
+      engine.runState(state);
+      Log.i(TAG, "Started " + state.getClass() + " in " + this.getClass());
+    }
 
     return state;
   }

TeamCode/src/main/java/org/samples/ConceptTensorFlowObjectDetectionWebcam.java

@@ -49,6 +49,7 @@ import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
  * is explained below.
  */
+@Disabled
 @TeleOp(name = "Concept: TensorFlow Object Detection Webcam", group = "Concept")
 
 public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode {

TeamCode/src/main/java/org/samples/ConceptVuMarkIdentificationWebcam.java

@@ -66,7 +66,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
  * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
  * is explained in {@link ConceptVuforiaNavigationWebcam}.
  */
-
+@Disabled
 @TeleOp(name="Concept: VuMark Id Webcam", group ="Concept")
 
 public class ConceptVuMarkIdentificationWebcam extends LinearOpMode {

TeamCode/src/main/java/org/samples/ConceptVuforiaUltimateGoalNavigationWebcam.java

@@ -84,7 +84,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocaliz
  * is explained below.
  */
 
-
+@Disabled
 @TeleOp(name="ULTIMATEGOAL Vuforia Nav Webcam", group ="Concept")
 
 public class ConceptVuforiaUltimateGoalNavigationWebcam extends LinearOpMode {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java

@@ -19,6 +19,6 @@ public class CalibrationEngine extends CyberarmEngine {
 
     @Override
     public void setup() {
-        addState(new CalibrateRingBeltLoop(robot));
+        addState(new OdometryCalibration(robot));
     }
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java

@@ -20,6 +20,8 @@ public class OdometryCalibration extends CyberarmState {
 
     @Override
     public void exec() {
+        robot.updateLocation();
+
         long timeCurrent = System.currentTimeMillis();
         timeChange = timeCurrent - timePrevious;
 
@@ -32,7 +34,7 @@ public class OdometryCalibration extends CyberarmState {
             rotation += robot.getRelativeAngle(imu, rotationPrev);
             rotationPrev = imu;
 
-            currentTick = (robot.encoderRight.getCurrentPosition() + robot.encoderLeft.getCurrentPosition()) / 2;
+            currentTick = (robot.encoderRight.getCurrentPosition() - robot.encoderLeft.getCurrentPosition()) / 2;
 
             if (engine.gamepad1.x) {
                 robot.setDrivePower(power, -power, power, -power);
@@ -55,9 +57,18 @@ public class OdometryCalibration extends CyberarmState {
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("degrees", rotation);
+        engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
-        engine.telemetry.addData("ticks", currentTick);
+        engine.telemetry.addData("Rotation ", robot.getRotation());
+//
+//        engine.telemetry.addData("degrees", rotation);
+        engine.telemetry.addData("ticks", robot.encoderRight.getCurrentPosition() );
         engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise);
         engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise);
+//        engine.telemetry.addData("forward", robot.forwardVector);
+//        engine.telemetry.addData("sideways", robot.sidewaysVector);
+    }
+
+    private float round(double number,double unit) {
+        return (float) (Math.floor(number/unit) * unit);
     }
 }

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

@@ -3,20 +3,54 @@ package org.timecrafters.UltimateGoal.Competition.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.cyberarm.engine.V2.CyberarmState;
 import org.timecrafters.UltimateGoal.Competition.Launch;
 import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.WobbleArm;
 import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
 
+import java.util.ArrayList;
+
 @Autonomous (name = "Autonomous")
 public class AutoEngine extends CyberarmEngine {
 
     private Robot robot;
     private TensorFlowCheck tensorFlowCheck;
 
+    private double launchTolerance;
+    private double launchPower;
+    private long launchBrakeTime;
+
+    private float scoreAFaceAngle;
+    private double scoreATolerance;
+    private double scoreAPower;
+    private long scoreABrakeTime;
+
+    float scoreBFaceAngle;
+    double scoreBTolerance;
+    double scoreBPower;
+    long scoreBBrakeTime;
+
     @Override
     public void init() {
         robot = new Robot(hardwareMap);
         robot.initHardware();
+        robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN);
+
+        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();
+
+//        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();
+//
+//        scoreBFaceAngle = robot.stateConfiguration.variable("auto","10_0","face").value();
+//        scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","10_0","tolPos").value());
+//        scoreBPower = robot.stateConfiguration.variable("auto","10_0","power").value();
+//        scoreBBrakeTime = robot.stateConfiguration.variable("auto","10_0","brakeMS").value();
+
         super.init();
     }
 
@@ -25,6 +59,9 @@ public class AutoEngine extends CyberarmEngine {
         //drive to view
         addState(new DriveToCoordinates(robot, "auto", "01_0"));
 
+        //face ring
+        addState(new Face(robot, "auto", "01_1"));
+
         //select scoring area
         tensorFlowCheck = new TensorFlowCheck(robot, "auto", "02_0");
         addState(tensorFlowCheck);
@@ -33,28 +70,30 @@ public class AutoEngine extends CyberarmEngine {
         addState(new DriveToCoordinates(robot, "auto", "03_0"));
 
         //drive to launch position
-        double launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
-        double launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
-        long launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","bakeMS").value();
         addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime));
 
+        //
+        addState(new Face(robot, "auto", "04_1"));
+
         //launch rings
-        addState(new Launch(robot));
+        addState(new Launch(robot, "auto", "04_2"));
 
         //drive to scoring area
-        float scoreAFaceAngle = robot.stateConfiguration.variable("auto","05_0","faceAngle").value();
+        addState(new DriveToCoordinates(robot, "auto", "05_0", true));
-        double scoreATolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value());
+//        addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime));
-        double scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value();
-        long scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","bakeMS").value();
-        addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime));
 
         //turn arm towards scoreing area.
-        addState(new Face(robot, "auto", "05_1"));
+        ArrayList<CyberarmState> threadStates = new ArrayList<>();
+         threadStates.add(new Face(robot, "auto", "05_1"));
+        threadStates.add(new WobbleArm(robot, "auto", "05_2",false));
+        addState(new ThreadStates(threadStates));
 
         //open the wobble grab arm
         addState(new WobbleGrab(robot, "auto", "06_0", true));
 
         //drive to second wobble
+        addState(new DriveToCoordinates(robot, "auto","06_1"));
+
         addState(new DriveToCoordinates(robot, "auto", "07_0"));
         addState(new DriveWithColorSensor(robot, "auto", "08_0"));
 
@@ -62,16 +101,18 @@ public class AutoEngine extends CyberarmEngine {
         addState(new WobbleGrab(robot, "auto", "09_0", false));
 
         //drive to scoring area
-        float scoreBFaceAngle = robot.stateConfiguration.variable("auto","05_0","faceAngle").value();
+//        addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime));
-        double scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value());
+        addState(new DriveToCoordinates(robot, "auto", "10_0", true));
-        double scoreBPower = robot.stateConfiguration.variable("auto","05_0","power").value();
-        long scoreBBrakeTime = robot.stateConfiguration.variable("auto","05_0","bakeMS").value();
-        addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime));
 
         //release wobble goal 2
-        addState(new WobbleGrab(robot, "auto", "10_0", true));
+        addState(new Face(robot, "auto", "11_0"));
+        addState(new WobbleGrab(robot, "auto", "12_0", true));
 
         //drive to park
-        addState(new DriveToCoordinates(robot, "auto", "11_0"));
+        ArrayList<CyberarmState> threadStatesB = new ArrayList<>();
+        threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false));
+        threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true));
+        threadStatesB.add(new DriveToCoordinates(robot, "auto", "13_0"));
+        addState(new ThreadStates(threadStatesB));
     }
 }

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

@@ -17,6 +17,7 @@ public class DriveToCoordinates extends CyberarmState {
     private long breakStartTime;
     private long brakeTime;
     private boolean autoFace;
+    private boolean scoringArea;
 
     public DriveToCoordinates(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -24,6 +25,13 @@ public class DriveToCoordinates extends CyberarmState {
         this.actionName = actionName;
     }
 
+    public DriveToCoordinates(Robot robot, String groupName, String actionName, boolean scoringArea) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+        this.scoringArea = scoringArea;
+    }
+
     public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime) {
         this.robot = robot;
         this.xTarget = xTarget;
@@ -37,8 +45,6 @@ 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();
@@ -53,9 +59,16 @@ public class DriveToCoordinates extends CyberarmState {
 
     @Override
     public void start() {
+        if (!groupName.equals("manual")) {
+            setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
 
-        if (robot.stateConfiguration.action(groupName,actionName).enabled) {
+            if (!scoringArea) {
-            setHasFinished(true);
+                xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
+                yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
+            } else {
+                xTarget = robot.wobbleScoreX;
+                yTarget = robot.wobbleScoreY;
+            }
         }
 
         if (autoFace) {
@@ -65,12 +78,14 @@ public class DriveToCoordinates extends CyberarmState {
 
     @Override
     public void exec() {
+        robot.updateLocation();
 
         double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
 
+        double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
+        robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
+
         if (distanceToTarget > tolerancePos) {
-            double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
-            robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
             braking = false;
         } else {
             long currentTime = System.currentTimeMillis();
@@ -78,7 +93,10 @@ public class DriveToCoordinates extends CyberarmState {
                 breakStartTime = currentTime;
                 braking = true;
             }
-            setHasFinished(currentTime - breakStartTime >= brakeTime);
+            if (currentTime - breakStartTime >= brakeTime) {
+                robot.setDrivePower(0,0,0,0);
+                setHasFinished(true);
+            }
         }
 
     }

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

@@ -26,6 +26,11 @@ public class DriveWithColorSensor extends CyberarmState {
         maximum = robot.stateConfiguration.variable(groupName,actionName,"max").value();
     }
 
+    @Override
+    public void start() {
+        setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
+    }
+
     @Override
     public void exec() {
         double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);

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

@@ -9,7 +9,7 @@ public class Face extends CyberarmState {
     private String groupName = "manual";
     private String actionName;
     private float faceAngle;
-    private double toleranceFace;
+    private float toleranceFace;
     private double power;
     private boolean braking;
     private long breakStartTime;
@@ -22,7 +22,7 @@ public class Face extends CyberarmState {
         this.actionName = actionName;
     }
 
-    public Face(Robot robot, float faceAngle, double toleranceFace, double power, long breakTime) {
+    public Face(Robot robot, float faceAngle, float toleranceFace, double power, long breakTime) {
         this.robot = robot;
         this.faceAngle = faceAngle;
         this.toleranceFace = toleranceFace;
@@ -32,14 +32,15 @@ public class Face extends CyberarmState {
 
     @Override
     public void init() {
+
         if (!groupName.equals("manual")) {
             power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
             toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
-            breakTime = robot.stateConfiguration.variable(groupName, actionName, "brake MS").value();
+            breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
 
             try {
                 faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value();
-            } catch (NullPointerException e) {
+            } catch (RuntimeException e) {
                 autoFace = true;
             }
         }
@@ -47,19 +48,25 @@ public class Face extends CyberarmState {
 
     @Override
     public void start() {
-        if (autoFace) {
+
-            double x = robot.stateConfiguration.variable(groupName, actionName, "faceX").value();
+        if (!groupName.equals("manual")) {
-            double y = robot.stateConfiguration.variable(groupName, actionName, "faceY").value();
+            setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
-            faceAngle = robot.getAngleToPosition(x,y);
+
+
+            if (autoFace) {
+                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.getAngleToPosition(x, y);
+            }
         }
     }
 
     @Override
     public void exec() {
-
+        robot.updateLocation();
+        double[] powers = robot.getFacePowers(faceAngle,power);
+        robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]);
         if (Math.abs(robot.getRelativeAngle(faceAngle,robot.getRotation())) > toleranceFace) {
-            double[] powers = robot.getFacePowers(faceAngle,power);
-            robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]);
             braking = false;
         } else {
             long currentTime = System.currentTimeMillis();
@@ -67,7 +74,16 @@ public class Face extends CyberarmState {
                 breakStartTime = currentTime;
                 braking = true;
             }
-            setHasFinished(currentTime - breakStartTime >= breakTime);
+            if (currentTime - breakStartTime >= breakTime) {
+                setHasFinished(true);
+                robot.setDrivePower(0,0,0,0);
+            }
         }
     }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("target angle", faceAngle);
+        engine.telemetry.addData("current angle", robot.getRotation());
+    }
 }

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

@@ -0,0 +1,56 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class FaceAndDrive extends CyberarmState {
+
+    private Robot robot;
+    private String groupName = "manual";
+    private String actionName;
+    private double xTarget;
+    private double yTarget;
+    private float faceAngle;
+    private double tolerancePos;
+    private double power;
+    private long brakeTime;
+    private float toleranceFace;
+
+    public FaceAndDrive(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+
+            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());
+            toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
+            brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
+
+            try {
+                faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
+            } catch (NullPointerException e) {
+                faceAngle = robot.getAngleToPosition(xTarget,yTarget);
+            }
+
+
+    }
+
+    @Override
+    public void start() {
+        addState(new Face(robot, faceAngle, toleranceFace, power, brakeTime));
+        addState(new DriveToCoordinates(robot,xTarget,yTarget,faceAngle,tolerancePos,power,brakeTime));
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(true);
+    }
+}

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

@@ -18,6 +18,7 @@ public class TensorFlowCheck extends CyberarmState {
     public double wobblePosX;
     public double wobblePosY;
     private int manualPath;
+    private String status;
 
     public TensorFlowCheck(Robot robot, String group, String action) {
         this.robot = robot;
@@ -33,47 +34,61 @@ public class TensorFlowCheck extends CyberarmState {
 
     @Override
     public void start() {
-
+        if (!robot.stateConfiguration.action(group,action).enabled) {
+            manualPath = -1;
+        } else {
+            robot.tfObjectDetector.activate();
+        }
     }
 
     @Override
     public void exec() {
 
-         if (manualPath == -1) {
+        if (runTime() < checkTime) {
-             recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
+            if (manualPath != -1) {
+                recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
 
-             if (recognitions != null) {
+                if (recognitions != null) {
+                    if (recognitions.size() == 1) {
+                        String label = recognitions.get(0).getLabel();
+                        if (label.equals("Single")) {
+                            path = 1;
+                        } else if (label.equals("Quad")) {
+                            path = 2;
+                        }
+                    } else if (recognitions.size() > 1) {
+                        path = 1;
+                    }
+                }
 
-                 if (recognitions.size() == 1) {
+            } else {
-                     Recognition recognition = recognitions.get(0);
+                path = manualPath;
-                     String label = recognition.getLabel();
+            }
-                     if (label.equals("Single")) {
+        } else {
-                         path = 1;
+            robot.tfObjectDetector.deactivate();
-                     } else if (label.equals("Quad")) {
-                         path = 2;
-                     }
-                 } else if (recognitions.size() > 1) {
-                     path = 1;
-                 }
-             }
-         } else {
-             path = manualPath;
-         }
 
-         if (runTime() >= checkTime) {
             if (path == 0) {
-                wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos0","x").value();
+                robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","x").value());
-                wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos0","y").value();
+                robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","y").value());
             }
             if (path == 1) {
-                 wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value();
+                robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","x").value());
-                 wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos1","y").value();
+                robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","y").value());
             }
              if (path == 2) {
-                 wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos2","x").value();
+                 robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","x").value());
-                 wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos2","y").value();
+                 robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","y").value());
              }
-         }
+             // make the servo look up once we're done using tensorFlow
+             robot.webCamServo.setPosition(0);
+             robot.launchMotor.setPower(Robot.LAUNCH_POWER);
 
+             setHasFinished(true);
+         }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Chosen Path", path);
     }
 }

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

@@ -6,7 +6,7 @@ import java.util.ArrayList;
 
 public class ThreadStates extends CyberarmState {
 
-    private ArrayList<CyberarmState> states = new ArrayList<>();
+    private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
 
     public ThreadStates(ArrayList<CyberarmState> states) {
         this.states = states;
@@ -30,4 +30,15 @@ public class ThreadStates extends CyberarmState {
         setHasFinished(finishedStates == states.size());
 
     }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Threaded States");
+        for (CyberarmState child: children) {
+            engine.telemetry.addLine(""+child.getClass());
+            if (child.getHasFinished()) {
+                engine.telemetry.addLine("finished");
+            }
+        }
+    }
 }

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

@@ -5,6 +5,8 @@ import org.cyberarm.engine.V2.CyberarmState;
 public class Launch extends CyberarmState {
 
     private Robot robot;
+    private String groupName;
+    private String actionName;
     boolean hasCycled;
     boolean detectedPass;
 
@@ -12,9 +14,23 @@ public class Launch extends CyberarmState {
         this.robot = robot;
     }
 
+    public Launch(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
     @Override
     public void start() {
-        robot.ringBeltOn();
+        try {
+            if (robot.stateConfiguration.action(groupName, actionName).enabled) {
+                robot.ringBeltOn();
+            } else {
+                setHasFinished(true);
+            }
+        } catch (NullPointerException e){
+            robot.ringBeltOn();
+        }
     }
 
     @Override

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

@@ -24,8 +24,11 @@ public class ProgressRingBelt extends CyberarmState {
             robot.ringBeltOn();
             robot.ringBeltStage += 1;
             prepLaunch = !robot.initLauncher;
+        } else if (robot.ringBeltStage > 2) {
+            setHasFinished(true);
         }
 
+
     }
 
     @Override

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

@@ -62,7 +62,7 @@ public class Robot {
     //Steering Constants
     static final double FINE_CORRECTION = 0.055 ;
     static final double LARGE_CORRECTION = 0.025;
-    static final double MOMENTUM_CORRECTION = 1.045;
+    static final double MOMENTUM_CORRECTION = 1.05;
     static final double MOMENTUM_MAX_CORRECTION = 1.4;
     static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
 
@@ -70,15 +70,20 @@ public class Robot {
     static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
     static final int COUNTS_PER_REVOLUTION = 8192;
     static final float mmPerInch = 25.4f;
+    static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD = 12.3;
+    static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD = 18.8;
     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  = 13f;
+    static final float CAMERA_FORWARD_DISPLACEMENT  = 8f;
     // Inches above Ground
-    static final float CAMERA_VERTICAL_DISPLACEMENT = 4.5f;
+    static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
     // Inches Left of axis of rotation
-    static final float CAMERA_LEFT_DISPLACEMENT     = 2f;
+    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
     public double locationX;
@@ -93,7 +98,7 @@ public class Robot {
 
     //Launcher
     public DcMotor launchMotor;
-    public static final double LAUNCH_POWER = 0.75;
+    public static final double LAUNCH_POWER = 0.7;
 
     private static final long LAUNCH_ACCEL_TIME = 500;
     public double launchPositionX;
@@ -126,13 +131,14 @@ public class Robot {
     public static final int WOBBLE_ARM_DOWN = -710;
     public static final double WOBBLE_SERVO_MAX = 0.3;
     public RevColorSensorV3 wobbleColorSensor;
+    public double wobbleScoreX;
+    public double wobbleScoreY;
 
     //vuforia navigation
     private WebcamName webcam;
     private VuforiaLocalizer vuforia;
     public Servo webCamServo;
-    public static final double CAM_SERVO_UP = 0.2;
+    public static final double CAM_SERVO_DOWN = 0.15;
-    public static final double CAM_SERVO_Down = 0.2;
 
     public boolean trackableVisible;
     private VuforiaTrackables targetsUltimateGoal;
@@ -151,11 +157,15 @@ public class Robot {
     public double totalV;
     public double visionX;
     public double visionY;
+    public double visionZ;
     public float rawAngle;
 //    private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
-    private String TestingRecord = "TicksPerDegree";
+    private String TestingRecord = "x,y";
 
-    public double traveledLeft;
+    public double forwardVector;
+    public double sidewaysVector;
+
+    public double traveledForward = 0;
     public double traveledRight;
 
     public void initHardware() {
@@ -192,6 +202,8 @@ public class Robot {
 
         wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
 
+        wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
+
         //init ring belt
         collectionMotor = hardwareMap.dcMotor.get("collect");
         collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -233,8 +245,8 @@ public class Robot {
         webCamServo.setDirection(Servo.Direction.REVERSE );
 
         rotation = stateConfiguration.variable("system", "startPos", "direction").value();
-        locationX = stateConfiguration.variable("system", "startPos", "x").value();
+        locationX = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "x").value());
-        locationY = stateConfiguration.variable("system", "startPos", "y").value();
+        locationY = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "y").value());
 
         minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value();
         minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value();
@@ -263,10 +275,11 @@ public class Robot {
             }
         }
         //
-        launchPositionX = stateConfiguration.variable("system", "launchPos","x").value();
+        launchPositionX = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","x").value());
-        launchPositionX = stateConfiguration.variable("system", "launchPos","y").value();
+        launchPositionY = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","y").value());
         launchRotation = stateConfiguration.variable("system", "launchPos","rot").value();
 
+        initTensorFlow();
     }
 
     private void initVuforia() {
@@ -338,7 +351,7 @@ public class Robot {
         double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
 
         int encoderLeftCurrent = -encoderLeft.getCurrentPosition();
-        int encoderRightCurrent = -encoderRight.getCurrentPosition();
+        int encoderRightCurrent = encoderRight.getCurrentPosition();
         int encoderBackCurrent = encoderBack.getCurrentPosition();
 
         double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
@@ -352,22 +365,27 @@ public class Robot {
 
         //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
-        //the two. This meathod doesn't require any prior calibration.
+        //the two.
-        double forwardVector = (encoderLeftChange+encoderRightChange)/2;
 
         //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
         //based on a separate calibration program.
 
-        double ticksPerDegree;
+        double ticksPerDegreeForward;
+        double ticksPerDegreeSideways;
 
         if (rotationChange < 0) {
-            ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
+            ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
+            ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD;
         } else {
-            ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
+            ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
+            ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD;
         }
 
-        double sidewaysVector = encoderBackChange + (rotationChange* ticksPerDegree);
+        forwardVector = ((encoderLeftChange+encoderRightChange)/2) -  (rotationChange* ticksPerDegreeForward);
+
+        traveledForward += forwardVector;
+        sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
 
         double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
         double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
@@ -420,13 +438,6 @@ public class Robot {
                     lastConfirmendLocation = robotLocation;
                 }
 
-
-                VectorF translation = lastConfirmendLocation.getTranslation();
-                locationX = -inchesToTicks(translation.get(1) / mmPerInch);
-                locationY = inchesToTicks( translation.get(0) / mmPerInch);
-
-
-
                 //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);
@@ -436,6 +447,16 @@ public class Robot {
                     this.rotation -= -180;
                 }
 
+                VectorF translation = lastConfirmendLocation.getTranslation();
+                double camX = -translation.get(1) / mmPerInch;
+                double camY = translation.get(0) / mmPerInch;
+
+                double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+
+                locationX = inchesToTicks(camX - displaceX);
+                locationY = inchesToTicks(camY - displaceY);
+
                 break;
             }
         }

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

@@ -27,6 +27,7 @@ public class TeleOpEngine extends CyberarmEngine {
     @Override
     public void stop() {
         robot.deactivateVuforia();
+        robot.saveRecording();
         super.stop();
     }
 }

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

@@ -14,9 +14,16 @@ public class TeleOpState extends CyberarmState {
     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;
@@ -33,7 +40,6 @@ public class TeleOpState extends CyberarmState {
     private boolean wobbleGrabOpen = false;
 
 
-
     private boolean launchInput = false;
 
 
@@ -45,11 +51,13 @@ public class TeleOpState extends CyberarmState {
     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 exec() {
         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;
@@ -60,9 +68,15 @@ public class TeleOpState extends CyberarmState {
         double rightJoystickX = engine.gamepad1.right_stick_x;
         double rightJoystickY = engine.gamepad1.right_stick_y;
 
-        rightJoystickDegrees = snapToCardinal((float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)),cardinalSnapping,0);
+        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) {
@@ -103,20 +117,14 @@ public class TeleOpState extends CyberarmState {
 
             //Normal Driver Controls
 
-            if (rightJoystickMagnitude == 0) {
+            if (leftJoystickMagnitude == 0) {
-                if (leftJoystickMagnitude !=0) {
+                double[] facePowers =  robot.getFacePowers(faceDirection,  TURN_POWER);
-                    float direction = snapToCardinal(leftJoystickDegrees,cardinalSnapping,0);
+                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
-                    powers = robot.getMecanumPowers(direction, drivePower, direction);
-                }
             } else {
-                if (leftJoystickMagnitude == 0) {
-                    double[] facePowers =  robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude);
-                    powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
-                } else {
 
-                    powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees,pairSnapping,rightJoystickDegrees), drivePower, rightJoystickDegrees);
+                powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees,pairSnapping,faceDirection), drivePower, faceDirection);
-                }
             }
+
         }
 
         robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
@@ -157,6 +165,10 @@ public class TeleOpState extends CyberarmState {
         bPrev = b;
 
 
+        if (engine.gamepad2.right_bumper && engine.gamepad2.left_bumper) {
+            robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+        }
+
     }
 
     @Override
@@ -164,13 +176,18 @@ public class TeleOpState extends CyberarmState {
 
        engine.telemetry.addData("childrenHaveFinished", childrenHaveFinished());
        for (CyberarmState state : children) {
-           engine.telemetry.addLine(""+state.getClass());
+           if (!state.getHasFinished()) {
+               engine.telemetry.addLine("" + state.getClass());
+           }
        }
 
+       engine.telemetry.addData("Vision Z", robot.visionZ);
+
+       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)+")");
         engine.telemetry.addData("Rotation ", robot.getRotation());
-        engine.telemetry.addData("totalV", robot.totalV);
     }
 
     private float round(double number,double unit) {

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

@@ -45,4 +45,8 @@ public class WobbleArm extends CyberarmState {
         setHasFinished(runTime() > waitTime);
     }
 
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("runTime", runTime());
+    }
 }

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

@@ -32,10 +32,14 @@ public class WobbleGrab extends CyberarmState {
 
     @Override
     public void start() {
-        if (open) {
+        if (robot.stateConfiguration.action(groupName, actionName).enabled) {
-            robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+            if (open) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+            } else {
+                robot.wobbleGrabServo.setPosition(0);
+            }
         } else {
-            robot.wobbleGrabServo.setPosition(0);
+            setHasFinished(true);
         }
     }