So many things

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

@@ -1,43 +0,0 @@
-package org.timecrafters.UltimateGoal.Calibration;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-
-import org.cyberarm.engine.V2.CyberarmState;
-
-public class Break extends CyberarmState {
-
-    public DcMotor driveBackLeft;
-    public DcMotor driveFrontLeft;
-    public DcMotor driveBackRight;
-    public DcMotor driveFrontRight;
-
-    @Override
-    public void init() {
-        driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft");
-        driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight");
-        driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft");
-        driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight");
-
-        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
-        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-    }
-
-    @Override
-    public void start() {
-
-    }
-
-    @Override
-    public void exec() {
-        driveFrontLeft.setPower(0);
-        driveFrontRight.setPower(0);
-        driveBackLeft.setPower(0);
-        driveBackRight.setPower(0);
-    }
-}

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

@@ -0,0 +1,58 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class CalibrateRingBeltLoop extends CyberarmState {
+
+    private Robot robot;
+    private float startingTick;
+    private int currentTick;
+    private double ticksPerLoop;
+    private boolean limit;
+    private boolean hasSeenLimit;
+    private boolean limitPrevious;
+
+    public CalibrateRingBeltLoop(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+
+        currentTick = robot.ringBeltMotor.getCurrentPosition();
+        limit = robot.limitSwitch.isPressed();
+
+        if (engine.gamepad1.x || (engine.gamepad1.a && !limit)) {
+            robot.ringBeltMotor.setPower(0.5);
+        } else {
+           robot.ringBeltMotor.setPower(0);
+        }
+
+        if (engine.gamepad1.y) {
+            robot.ringBeltMotor.setPower(-0.1);
+        } else {
+            robot.ringBeltMotor.setPower(0);
+        }
+
+        if (engine.gamepad1.b) {
+            startingTick = currentTick;
+        }
+
+        if (limit && !limitPrevious){
+            if (hasSeenLimit) {
+                ticksPerLoop = currentTick - startingTick;
+            }
+            startingTick = currentTick;
+            hasSeenLimit = true;
+        }
+        limitPrevious = limit;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("ticks", currentTick - startingTick);
+        engine.telemetry.addData("limit swich", limit );
+        engine.telemetry.addData("Clockwise", ticksPerLoop);
+    }
+}

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 VelocityTest(robot));
+        addState(new CalibrateRingBeltLoop(robot));
     }
 }

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

@@ -0,0 +1,59 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Engagable;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.DriveMotor;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class MotorCheck extends CyberarmState {
+
+    private Robot robot;
+    private double[] Powers;
+    private DcMotor[] Motors;
+    private int currentMotor = 0;
+    private double currentPower = 0.001;
+
+    private boolean yPrevious;
+
+
+    public MotorCheck(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
+    }
+
+    @Override
+    public void start() {
+        Motors[currentMotor].setPower(0.2);
+    }
+
+    @Override
+    public void exec() {
+
+        if (currentMotor <= 3) {
+
+            boolean y = engine.gamepad1.y;
+            if (y && !yPrevious) {
+
+                Motors[currentMotor].setPower(0);
+                currentMotor += 1;
+                Motors[currentMotor].setPower(0.2);
+                sleep(20);
+            }
+            yPrevious = y;
+        }
+
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addData("current Motor", Motors[currentMotor].toString());
+
+    }
+}

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

@@ -6,10 +6,13 @@ import org.timecrafters.UltimateGoal.Competition.Robot;
 public class OdometryCalibration extends CyberarmState {
 
     private Robot robot;
-    private float rotation;
+    private float rotation = 0;
+    private float rotationPrev = 0;
     private int currentTick;
     private double ticksPerDegreeClockwise;
     private double ticksPerDegreeCounterClockwise;
+    private long timePrevious;
+    private long timeChange;
 
     public OdometryCalibration(Robot robot) {
         this.robot = robot;
@@ -17,21 +20,37 @@ public class OdometryCalibration extends CyberarmState {
 
     @Override
     public void exec() {
+        long timeCurrent = System.currentTimeMillis();
+        timeChange = timeCurrent - timePrevious;
 
-        double power = 0.1;
+        if (timeChange >= 1200) {
-        rotation = -robot.imu.getAngularOrientation().firstAngle;
+            timePrevious = timeCurrent;
-        currentTick = robot.encoderBack.getCurrentPosition();
 
-        if (engine.gamepad1.x) {
+
-            robot.setDrivePower(power, -power, power, -power);
+            double power = 0.25;
-            ticksPerDegreeClockwise = currentTick/rotation;
+            float imu = -robot.imu.getAngularOrientation().firstAngle;
-        } else if(engine.gamepad1.y) {
+            rotation += robot.getRelativeAngle(imu, rotationPrev);
-            robot.setDrivePower(-power, power, -power, power);
+            rotationPrev = imu;
-            ticksPerDegreeCounterClockwise = currentTick/rotation;
+
-        } else {
+            currentTick = robot.encoderBack.getCurrentPosition();
-            robot.setDrivePower(0,0,0,0);
+
+            if (engine.gamepad1.x) {
+                robot.setDrivePower(power, -power, power, -power);
+                ticksPerDegreeClockwise = currentTick / rotation;
+            } else if (engine.gamepad1.y) {
+                robot.setDrivePower(-power, power, -power, power);
+                ticksPerDegreeCounterClockwise = currentTick / rotation;
+            } else {
+                robot.setDrivePower(0, 0, 0, 0);
+            }
         }
 
+        if (engine.gamepad1.b) {
+            robot.record("Clockwise : "+ticksPerDegreeClockwise);
+            robot.record("Counter Clockwise : "+ticksPerDegreeCounterClockwise);
+            robot.saveRecording();
+            setHasFinished(true);
+        }
     }
 
     @Override

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

@@ -19,7 +19,7 @@ public class PerformanceTest extends CyberarmState {
     @Override
     public void exec() {
         robot.updateLocation();
-        robot.record();
+//        robot.record();
 
         if (engine.gamepad1.x) {
             double[] powers = robot.getMecanumPowers(0, 1, 0);

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

@@ -1,70 +0,0 @@
-package org.timecrafters.UltimateGoal.Calibration;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Competition.Robot;
-
-public class StalPowerCalibrator extends CyberarmState {
-
-    private Robot robot;
-    private double[] Powers;
-    private DcMotor[] Motors;
-    private int currentMotor = 0;
-    private double currentPower = 0.001;
-
-
-
-    public StalPowerCalibrator(Robot robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void init() {
-//        Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
-        Powers = new double[] {0,0,0,0};
-
-        for (DcMotor motor : Motors) {
-            motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        }
-    }
-
-    @Override
-    public void exec() {
-
-        if (currentMotor <= 3) {
-
-
-
-            if (Math.abs(Motors[currentMotor].getCurrentPosition()) < 10 && currentPower < 1) {
-                currentPower += 0.001;
-                Motors[currentMotor].setPower(currentPower);
-                sleep(20);
-            } else {
-                Powers[currentMotor] = currentPower;
-                Motors[currentMotor].setPower(0);
-                currentPower = 0.001;
-                currentMotor += 1;
-                sleep(1000);
-                for (DcMotor motor : Motors) {
-                    motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-                    motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-                }
-            }
-        }
-
-    }
-
-    @Override
-    public void telemetry() {
-
-        engine.telemetry.addData("current Power", currentPower);
-        engine.telemetry.addData("current Motor", currentMotor);
-
-        engine.telemetry.addData("FrontLeft", Powers[0]);
-        engine.telemetry.addData("FrontRight", Powers[1]);
-        engine.telemetry.addData("BackLeft", Powers[2]);
-        engine.telemetry.addData("BackRight", Powers[3]);
-    }
-}

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

@@ -1,49 +0,0 @@
-package org.timecrafters.UltimateGoal.Calibration;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Competition.DriveMotor;
-import org.timecrafters.UltimateGoal.Competition.Robot;
-
-public class VelocityTest extends CyberarmState {
-
-    private Robot robot;
-    private double[] Velocities;
-    private DriveMotor[] Motors;
-    private long LastTime = 0;
-
-
-
-    public  VelocityTest(Robot robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void init() {
-        Motors = new DriveMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
-        Velocities = new double[] {0,0,0,0};
-
-    }
-
-    @Override
-    public void start() {
-        for (DriveMotor motor : Motors) {
-            motor.setPower(0.1);
-        }
-    }
-
-    @Override
-    public void exec() {
-
-        for (int i = 0; i < 4; i++) {
-            Velocities[i] = (Motors[i].motor.getCurrentPosition()* 60000) / runTime();
-        }
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("FrontLeft", Velocities[0]);
-        engine.telemetry.addData("FrontRight", Velocities[1]);
-        engine.telemetry.addData("BackLeft", Velocities[2]);
-        engine.telemetry.addData("BackRight", Velocities[3]);
-    }
-}

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

@@ -1,4 +1,27 @@
 package org.timecrafters.UltimateGoal.Competition.Autonomous;
 
-public class AutoEngine {
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+@Autonomous (name = "Autonomous")
+public class AutoEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        //drive to view
+        addState(new DriveToCoordinates(robot, "auto", "001_0"));
+        addState(new DriveToCoordinates(robot, "auto", "001_0"));
+
+    }
 }

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

@@ -11,11 +11,12 @@ public class DriveToCoordinates extends CyberarmState {
     private double xTarget;
     private double yTarget;
     private float faceAngle;
-    private double tolerance;
+    private double tolerancePos;
     private double power;
     private boolean braking;
     private long breakStartTime;
     private long breakTime;
+    private boolean autoFace;
 
     public DriveToCoordinates(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -23,11 +24,12 @@ public class DriveToCoordinates extends CyberarmState {
         this.actionName = actionName;
     }
 
-    public DriveToCoordinates(double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) {
+    public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) {
+        this.robot = robot;
         this.xTarget = xTarget;
         this.yTarget = yTarget;
         this.faceAngle = faceAngle;
-        this.tolerance = tolerance;
+        this.tolerancePos = tolerance;
         this.power = power;
         this.breakTime = breakTime;
     }
@@ -35,18 +37,25 @@ public class DriveToCoordinates extends CyberarmState {
     @Override
     public void init() {
         if (!groupName.equals("manual")) {
-            xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "x").value());
+            xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
-            yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "y").value());
+            yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
             power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
-            faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
+            tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
-            tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tol").value());
+            breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
-            breakTime = robot.stateConfiguration.variable(groupName, actionName, "breakMS").value();
+
+            try {
+                faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
+            } catch (NullPointerException e) {
+                autoFace = true;
+            }
         }
     }
 
     @Override
     public void start() {
-
+        if (autoFace) {
+            faceAngle = robot.getAngleToPosition(xTarget,yTarget);
+        }
     }
 
     @Override
@@ -54,18 +63,17 @@ public class DriveToCoordinates extends CyberarmState {
 
         double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
 
-        if (distanceToTarget > tolerance) {
+        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();
             if (!braking) {
-                breakStartTime = System.currentTimeMillis();
+                breakStartTime = currentTime;
-                robot.setBrakePosAll();
                 braking = true;
             }
-            robot.brakeAll();
+            setHasFinished(currentTime - breakStartTime >= breakTime);
-            setHasFinished(System.currentTimeMillis() - breakStartTime > breakTime);
         }
 
     }

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

@@ -1,66 +0,0 @@
-package org.timecrafters.UltimateGoal.Competition.Autonomous;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Competition.Robot;
-
-public class DriveToPosition extends CyberarmState {
-
-    private Robot robot;
-    private String groupName;
-    private String actionName;
-    private double targetX;
-    private double targetY;
-    private double translation;
-    private double tolerance;
-    private double power;
-
-    public DriveToPosition(Robot robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.groupName = groupName;
-        this.actionName = actionName;
-    }
-
-    @Override
-    public void init() {
-//        translation = robot.inchesToTicks(4);
-        targetX = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"x").value());
-        targetY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"y").value());
-        tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"tolerance").value());
-        power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
-    }
-
-    @Override
-    public void start() {
-
-    }
-
-    @Override
-    public void exec() {
-        robot.updateLocation();
-//        robot.record();
-
-        double distanceRemaining = Math.hypot(targetX - robot.getLocationX(), targetY - robot.getLocationY());
-
-        //Math.pow(3, distanceRemaining - translation) + 0.3;
-
-//            if (power > 0.65) {
-//                power = 0.65;
-//            }
-
-        if (distanceRemaining < tolerance) {
-            robot.setDrivePower(0, 0, 0, 0);
-            setHasFinished(true);
-        } else {
-//            robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);
-        }
-
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("Target Visible", robot.trackableVisible);
-        engine.telemetry.addData("Odo X", robot.ticksToInches(robot.getLocationX()));
-        engine.telemetry.addData("Odo Y", robot.ticksToInches(robot.getLocationY()));
-        engine.telemetry.addData(" Rotation", robot.getRotation());
-    }
-}

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

@@ -0,0 +1,73 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Face extends CyberarmState {
+
+    private Robot robot;
+    private String groupName = "manual";
+    private String actionName;
+    private float faceAngle;
+    private double toleranceFace;
+    private double power;
+    private boolean braking;
+    private long breakStartTime;
+    private long breakTime;
+    private boolean autoFace;
+
+    public Face(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    public Face(Robot robot, float faceAngle, double toleranceFace, double power, long breakTime) {
+        this.robot = robot;
+        this.faceAngle = faceAngle;
+        this.toleranceFace = toleranceFace;
+        this.power = power;
+        this.breakTime = breakTime;
+    }
+
+    @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();
+
+            try {
+                faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value();
+            } catch (NullPointerException e) {
+                autoFace = true;
+            }
+        }
+    }
+
+    @Override
+    public void start() {
+        if (autoFace) {
+            double x = robot.stateConfiguration.variable(groupName, actionName, "faceX").value();
+            double y = robot.stateConfiguration.variable(groupName, actionName, "faceY").value();
+            faceAngle = robot.getAngleToPosition(x,y);
+        }
+    }
+
+    @Override
+    public void exec() {
+
+        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();
+            if (!braking) {
+                breakStartTime = currentTime;
+                braking = true;
+            }
+            setHasFinished(currentTime - breakStartTime >= breakTime);
+        }
+    }
+}

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

@@ -0,0 +1,59 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class TensorFlowCheck extends CyberarmState {
+
+    private Robot robot;
+    private String group;
+    private String action;
+    private List<Recognition> recognitions;
+    private int path = 0;
+    private long checkTime;
+    public double wobblePosX;
+    public double wobblePosY;
+
+
+
+    @Override
+    public void init() {
+        checkTime = robot.stateConfiguration.variable(group,action,"time").value();
+    }
+
+    @Override
+    public void start() {
+
+    }
+
+    @Override
+    public void exec() {
+         recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
+
+         if (recognitions != null) {
+
+             if (recognitions.size() == 1) {
+                 Recognition recognition = recognitions.get(0);
+                 String label = recognition.getLabel();
+                 if (label.equals("Single")) {
+                    path = 1;
+                 } else if (label.equals("Quad")) {
+                    path = 2;
+                 }
+             } else if (recognitions.size() > 1) {
+                path = 1;
+             }
+         }
+
+         if (runTime() >= checkTime) {
+            if (checkTime == 0) {
+                wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value();
+            }
+         }
+
+    }
+}

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

@@ -0,0 +1,47 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class CamServo extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private boolean open;
+    private long waitTime = -1;
+
+    public CamServo(Robot robot, String groupName, String actionName, boolean open) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+        this.open = open;
+    }
+
+    public CamServo(Robot robot, boolean armUp, long waitTime) {
+        this.robot = robot;
+        this.open = armUp;
+        this.waitTime = waitTime;
+    }
+
+    @Override
+    public void init() {
+        if (waitTime == -1) {
+            waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
+        }
+    }
+
+    @Override
+    public void start() {
+        if (open) {
+            robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+        } else {
+            robot.wobbleGrabServo.setPosition(0);
+        }
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(runTime() > waitTime);
+    }
+
+}

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

@@ -10,7 +10,7 @@ public class DriveMotor {
     private int brakePosition;
     private MotorConfigurationType motorType;
     private DcMotorSimple.Direction direction;
-    static final double FINE_CORRECTION = 0.02;
+    static final double FINE_CORRECTION = 0.05;
     static final double LARGE_CORRECTION = 0.03;
 
     public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) {
@@ -20,9 +20,9 @@ public class DriveMotor {
     }
 
     public void init() {
-        motor.setMotorType(motorType);
+//        motor.setMotorType(motorType);
-        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+//        motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         motor.setDirection(direction);
     }
 

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

@@ -1,4 +1,39 @@
 package org.timecrafters.UltimateGoal.Competition;
 
-public class Launch {
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Launch extends CyberarmState {
+
+    private Robot robot;
+    boolean hasCycled;
+    boolean detectedPass;
+
+    public Launch(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        robot.ringBeltOn();
+    }
+
+    @Override
+    public void exec() {
+        //detect when limit switch is initially triggered
+        boolean detectingPass = robot.limitSwitch.isPressed();
+        if (detectingPass && !detectedPass) {
+            //finish once the ring belt has cycled all the way through and then returned to
+            //the first receiving position.
+            if (hasCycled) {
+                robot.ringBeltMotor.setPower(0);
+                robot.ringBeltStage = 0;
+                setHasFinished(true);
+            } else {
+                hasCycled = true;
+            }
+        }
+        detectedPass = detectingPass;
+
+//        if (robot.getBeltPos() > robot.loopPos(Robot.RING_BELT_GAP * 3) && hasCycled);
+    }
 }

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

@@ -0,0 +1,26 @@
+package org.timecrafters.UltimateGoal.Competition.PreInit;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class FindLimitSwitch extends CyberarmState {
+
+    private Robot robot;
+
+    public FindLimitSwitch(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        robot.ringBeltOn();
+    }
+
+    @Override
+    public void exec() {
+        if (robot.limitSwitch.isPressed()) {
+            robot.ringBeltMotor.setPower(0);
+            setHasFinished(true);
+        }
+    }
+}

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

@@ -0,0 +1,30 @@
+package org.timecrafters.UltimateGoal.Competition.PreInit;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class LoadRings extends CyberarmState {
+
+    private Robot robot;
+    private ProgressRingBelt ringBeltState;
+    private int ringCount = 0;
+
+    public LoadRings(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        robot.collectionMotor.setPower(1);
+    }
+
+    @Override
+    public void exec() {
+        ringBeltState = new ProgressRingBelt(robot);
+        if (engine.gamepad1.x && childrenHaveFinished()) {
+            addParallelState(ringBeltState);
+
+        }
+    }
+}

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

@@ -0,0 +1,25 @@
+package org.timecrafters.UltimateGoal.Competition.PreInit;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+@Autonomous (name = "Load Rings")
+public class PreInitEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new FindLimitSwitch(robot));
+        addState(new LoadRings(robot));
+    }
+}

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

@@ -0,0 +1,49 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class ProgressRingBelt extends CyberarmState {
+
+    private Robot robot;
+    private int targetPos;
+    private boolean useLaunchPrep;
+
+    public ProgressRingBelt(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        useLaunchPrep = (robot.launchMotor.getPower() == 0);
+    }
+
+    @Override
+    public void start() {
+        int currentPos = robot.getBeltPos();
+        if (robot.ringBeltStage < 2) {
+            targetPos = robot.loopPos(currentPos + Robot.RING_BELT_GAP);
+            robot.ringBeltOn();
+            robot.ringBeltStage += 1;
+        } else if (robot.ringBeltStage == 2) {
+            targetPos = robot.loopPos(currentPos + 160);
+            robot.ringBeltOn();
+            robot.ringBeltStage += 1;
+        }
+
+    }
+
+    @Override
+    public void exec() {
+        int currentPos = robot.getBeltPos();
+        if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) {
+            robot.ringBeltMotor.setPower(0);
+
+            if(useLaunchPrep) {
+                robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+            }
+
+            setHasFinished(true);
+        }
+    }
+
+}

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

@@ -4,10 +4,12 @@ import android.os.Environment;
 import android.util.Log;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.hardware.rev.RevTouchSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
 import org.firstinspires.ftc.robotcore.external.ClassFactory;
@@ -35,7 +37,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
 import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX;
 import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
 
-public class   Robot {
+public class Robot {
 
     private HardwareMap hardwareMap;
 
@@ -46,34 +48,29 @@ public class   Robot {
     public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
     public BNO055IMU imu;
 
-    private WebcamName webcam;
-    private VuforiaLocalizer vuforia;
-
     //drive system
-
+    public DcMotor driveFrontLeft;
-    public DriveMotor driveFrontLeft;
+    public DcMotor driveBackLeft;
-    public DriveMotor driveBackLeft;
+    public DcMotor driveFrontRight;
-    public DriveMotor driveFrontRight;
+    public DcMotor driveBackRight;
-    public DriveMotor driveBackRight;
 
     public DcMotor encoderLeft;
     public DcMotor encoderRight;
     public DcMotor encoderBack;
 
     //Steering Constants
-    static final double FINE_CORRECTION = 0.05 ;
+    static final double FINE_CORRECTION = 0.055 ;
-    static final double LARGE_CORRECTION = 0.002;
+    static final double LARGE_CORRECTION = 0.025   ;
-    static final double MOMENTUM_CORRECTION = 1.015;
+    static final double MOMENTUM_CORRECTION = 1.02;
-    static final double MOMENTUM_MAX_CORRECTION = 1.35;
+    static final double MOMENTUM_MAX_CORRECTION = 1.3;
     static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
 
     //Conversion Constants
-    static final double ENCODER_CIRCUMFERENCE = Math.PI * 4;
+    static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
     static final int COUNTS_PER_REVOLUTION = 8192;
     static final float mmPerInch = 25.4f;
-    //todo make/run calibration code to find this exact value
+    static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4;
-    static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = Math.PI * 1000;
+    static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6;
-    static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = Math.PI * 1000;
 
     // Inches Forward of axis of rotation
     static final float CAMERA_FORWARD_DISPLACEMENT  = 13f;
@@ -95,85 +92,117 @@ public class   Robot {
 
     //Launcher
     public DcMotor launchMotor;
+    public static final double LAUNCH_POWER = 0.75;
 
-    private static final long LAUNCH_ACCEL_TIME = 3000;
+    private static final long LAUNCH_ACCEL_TIME = 500;
-    public static final double LAUNCH_POSITION_X = 28 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
+    public static final double LAUNCH_POSITION_X = 36 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
-    public static final double LAUNCH_POSITION_Y = 0 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
+    public static final double LAUNCH_POSITION_Y = -8 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
     public static final float LAUNCH_ROTATION = 0;
-    public static final double LAUNCH_TOLERANCE_POS = 50;
+    public static final double LAUNCH_TOLERANCE_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
     public static final double LAUNCH_TOLERANCE_FACE = 0.5;
 
-    //Ring Belt
-    public DcMotor collectionMotor;
-    public DcMotor ringBeltMotor;
-    private DigitalChannel limitSwitch;
 
-    //todo: tune these when they exist
+
-    public static final int RING_BELT_LOOP_TICKS = 1000;
+    //Ring Intake
-    public static final int RING_BELT_GAP = 200;
+    public DcMotor collectionMotor;
-    public static final int RING_BELT_RECEIVE_POS = 100;
+    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 static final int RING_BELT_GAP = 670;
 
-    //Debugging
+    public static final double RING_BELT_POWER = 0.2;
-    public double visionX;
-    public double visionY;
-    public float rawAngle;
-//    private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
-    private String TestingRecord = "AngularVelocity";
 
-    public double traveledLeft;
+    //Wobble Goal Arm
-    public double traveledRight;
+    public DcMotor wobbleArmMotor;
+    public Servo  wobbleGrabServo;
+    public static final int WOBBLE_ARM_DOWN = -710;
 
+    public static final double WOBBLE_SERVO_MAX = 0.3;
 
     //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.2;
+
     public boolean trackableVisible;
     private VuforiaTrackables targetsUltimateGoal;
     private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
     private OpenGLMatrix lastConfirmendLocation;
 
     private long timeStartZeroVelocity = 0;
-    private static final long MIN_CHECK_DURATION_MS = 500;
+    private long minCheckDurationMs = 500;
-    private static final double MIN_CHECK_VELOCITY = 0.005;
+    private int minCheckVelocity = 1;
 
     //TensorFlow Object Detection
     public TFObjectDetector tfObjectDetector;
     private static final float MINIMUM_CONFIDENCE = 0.8f;
 
+    //Debugging
+    public double totalV;
+    public double visionX;
+    public double visionY;
+    public float rawAngle;
+//    private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
+    private String TestingRecord = "TicksPerDegree";
+
+    public double traveledLeft;
+    public double traveledRight;
 
     public void initHardware() {
-//        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
-        imu  = hardwareMap.get(BNO055IMU.class, "imu");
+        limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
-//        limitSwitch.setMode(DigitalChannel.Mode.INPUT);
 
-        DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft");
+        driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
+        driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
+        driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
+        driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
 
-        MotorConfigurationType motorType = dcMotor.getMotorType();
+        driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
-        motorType.setGearing(5);
+        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
-        motorType.setTicksPerRev(140);
+        driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
-        motorType.setMaxRPM(800);
+        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        driveFrontLeft = new DriveMotor(dcMotor, motorType, DcMotorSimple.Direction.FORWARD);
+        //Localization encoders
-        driveFrontRight = new DriveMotor(hardwareMap.dcMotor.get("driveFrontRight"), motorType, DcMotorSimple.Direction.REVERSE);
+        encoderRight = hardwareMap.dcMotor.get("driveFrontRight");
-        driveBackLeft = new DriveMotor(hardwareMap.dcMotor.get("driveBackLeft"), motorType, DcMotorSimple.Direction.FORWARD);
+        encoderBack = hardwareMap.dcMotor.get("driveFrontLeft");
-        driveBackRight = new DriveMotor(hardwareMap.dcMotor.get("driveBackRight"), motorType, DcMotorSimple.Direction.REVERSE);
+        encoderLeft = hardwareMap.dcMotor.get("collect");
 
-        driveFrontLeft.init();
+        encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        driveFrontRight.init();
+        encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        driveBackLeft.init();
+        encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        driveBackRight.init();
+        encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-//        encoderLeft = hardwareMap.dcMotor.get("odoLeft");
+        //init wobble arm
-//        encoderRight = hardwareMap.dcMotor.get("odoRight");
+        wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm");
-//        encoderBack = hardwareMap.dcMotor.get("odoBack");
+        wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        wobbleArmMotor.setTargetPosition(0);
+        wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
-        launchMotor = hardwareMap.dcMotor.get("launcher");
+        wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
+
+        //init ring belt
         collectionMotor = hardwareMap.dcMotor.get("collect");
-        collectionMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        //init ring belt
         ringBeltMotor = hardwareMap.dcMotor.get("belt");
+        ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+
+        //init IMU
+        imu  = hardwareMap.get(BNO055IMU.class, "imu");
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
@@ -196,18 +225,39 @@ public class   Robot {
 
         imu.startAccelerationIntegration(startPosition,startVelocity, 10);
 
-//        initVuforia();
+        //Init Localization
+        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+        initVuforia();
+        webCamServo = hardwareMap.servo.get("look");
+        webCamServo.setDirection(Servo.Direction.REVERSE );
 
         rotation = stateConfiguration.variable("system", "startPos", "direction").value();
         locationX = stateConfiguration.variable("system", "startPos", "x").value();
         locationY = stateConfiguration.variable("system", "startPos", "y").value();
 
-//        double launcherPower = 0;
+        minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value();
-//        long launchAccelStart = System.currentTimeMillis();
+        minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value();
-//        while (launcherPower < 1) {
+
-//            launcherPower = (double) (System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME;
+        //Init Launch Motor
-//            launchMotor.setPower(launcherPower);
+        DcMotor launcher = hardwareMap.dcMotor.get("launcher");
-//        }
+
+        MotorConfigurationType launchMotorType = launcher.getMotorType();
+        launchMotorType.setGearing(3);
+        launchMotorType.setTicksPerRev(84);
+        launchMotorType.setMaxRPM(2400);
+
+        launchMotor = launcher;
+        launchMotor.setMotorType(launchMotorType);
+        launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        if (stateConfiguration.action("system","initLauncher").enabled) {
+            double launcherPower = 0;
+            long launchAccelStart = System.currentTimeMillis();
+            while (launcherPower < LAUNCH_POWER) {
+                launcherPower = (double) ((System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME) * LAUNCH_POWER;
+                launchMotor.setPower(launcherPower);
+            }
+        }
 
     }
 
@@ -267,7 +317,7 @@ public class   Robot {
         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
                 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
         TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
-        parameters.minResultConfidence = MINIMUM_CONFIDENCE;
+        parameters.minResultConfidence = stateConfiguration.variable("system", "tensorFlow", "minConfidence").value();
         tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
         tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
     }
@@ -279,9 +329,8 @@ public class   Robot {
         float imuAngle = -imu.getAngularOrientation().firstAngle;
         double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
 
-        //todo add this when encoders exist.
+        int encoderLeftCurrent = -encoderLeft.getCurrentPosition();
-        int encoderLeftCurrent = encoderLeft.getCurrentPosition();
+        int encoderRightCurrent = -encoderRight.getCurrentPosition();
-        int encoderRightCurrent = encoderRight.getCurrentPosition();
         int encoderBackCurrent = encoderBack.getCurrentPosition();
 
         double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
@@ -309,8 +358,8 @@ public class   Robot {
         } else {
             ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
         }
-        //todo:  check if direction of the back encoder gives expected values
+
-        double sidewaysVector = encoderBackChange - (rotationChange* ticksPerDegree);
+        double sidewaysVector = encoderBackChange + (rotationChange* ticksPerDegree);
 
         double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
         double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
@@ -324,14 +373,15 @@ public class   Robot {
         rotation += rotationChange;
 
 
-        double totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightCurrent) + Math.abs(encoderBackChange);
+        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
 
-        if (totalV < MIN_CHECK_VELOCITY) {
+
+        if (totalV < minCheckVelocity) {
             long timeCurrent = System.currentTimeMillis();
 
             if (timeStartZeroVelocity == 0) {
                 timeStartZeroVelocity = timeCurrent;
-            } else if (timeCurrent - timeStartZeroVelocity >= MIN_CHECK_DURATION_MS) {
+            } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) {
                 syncWithVuforia();
             }
 
@@ -349,15 +399,6 @@ public class   Robot {
 
     }
 
-    public void checkVuforiaSync() {
-
-        double V1 = driveFrontLeft.motor.getPower();
-        double V2 = driveFrontRight.motor.getPower();
-        double V3 = driveBackLeft.motor.getPower();
-        double V4 = driveBackRight.motor.getPower();
-
-    }
-
     public void syncWithVuforia() {
         trackableVisible = false;
         for (VuforiaTrackable trackable : trackables) {
@@ -480,23 +521,23 @@ public class   Robot {
         double powerBackRight = scalar * (p + turnCorrection);
         double powerBackLeft = scalar * (q - turnCorrection);
 
-        //the turnCorrection often results in powers with magnitudes significantly larger than the
+//        //the turnCorrection often results in powers with magnitudes significantly larger than the
-        //scalar. The scaleRatio mitigates this without altering the quality of the motion by making
+//        //scalar. The scaleRatio mitigates this without altering the quality of the motion by making
-        //it so that the average of the four magnitudes is equal to the scalar magnitude.
+//        //it so that the average of the four magnitudes is equal to the scalar magnitude.
-        double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) +
+//        double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) +
-                Math.abs(powerBackRight) + Math.abs(powerBackLeft);
+//                Math.abs(powerBackRight) + Math.abs(powerBackLeft);
-        double scaleRatio = (4 * Math.abs(scalar))/powerSum;
+//        double scaleRatio = (4 * Math.abs(scalar))/powerSum;
-
+//
-        powerForwardRight *= scaleRatio;
+//        powerForwardRight *= scaleRatio;
-        powerForwardLeft *= scaleRatio;
+//        powerForwardLeft *= scaleRatio;
-        powerBackRight *= scaleRatio;
+//        powerBackRight *= scaleRatio;
-        powerBackLeft *= scaleRatio;
+//        powerBackLeft *= scaleRatio;
 
 
         if (relativeRotation != 0) {
             double momentumRelative =  angularVelocity * (relativeRotation / Math.abs(relativeRotation));
-            double exponential = Math.pow(MOMENTUM_CORRECTION, -momentumRelative);
+            double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
-            double momentumCorrection = (2*exponential)/(1+exponential);
+            double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
             powerForwardRight *= momentumCorrection;
             powerForwardLeft *= momentumCorrection;
             powerBackRight *= momentumCorrection;
@@ -532,21 +573,6 @@ public class   Robot {
         return powers;
     }
 
-    public void brakeAll() {
-        driveFrontLeft.brake();
-        driveFrontRight.brake();
-        driveBackLeft.brake();
-        driveBackRight.brake();
-    }
-
-    //sets the position the motors will lock to when braking to the motor's current position.
-    public void setBrakePosAll() {
-        driveFrontLeft.setBrakePosition();
-        driveFrontRight.setBrakePosition();
-        driveBackLeft.setBrakePosition();
-        driveBackRight.setBrakePosition();
-    }
-
     //Outputs the power necessary to turn and face a provided direction
     public double[] getFacePowers(float direction, double power) {
         angularVelocity = imu.getAngularVelocity().xRotationRate;
@@ -568,20 +594,21 @@ public class   Robot {
         return powers;
     }
 
+    public void ringBeltOn() {
+        ringBeltMotor.setPower(RING_BELT_POWER);
+    }
+
     public int getBeltPos(){
         return loopPos(ringBeltMotor.getCurrentPosition());
     }
 
     public int loopPos(int pos) {
-        if (pos < 0) {
-         pos += RING_BELT_LOOP_TICKS;
-        }
         pos %= RING_BELT_LOOP_TICKS;
         return pos;
     }
 
-    public void record() {
+    public void record(String record) {
-        TestingRecord+="\n"+angularVelocity;
+        TestingRecord+="\n"+record;
     }
 
     public void saveRecording() {

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

@@ -1,10 +1,32 @@
 package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
-import org.cyberarm.engine.V2.CyberarmEngine;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+@TeleOp (name = "TeleOp",group = "comp")
 public class TeleOpEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(0);
+        robot.webCamServo.setPosition(0);
+        super.init();
+    }
+
     @Override
     public void setup() {
+        addState(new TeleOpState(robot));
+    }
 
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+        super.stop();
     }
 }

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

@@ -1,8 +1,11 @@
 package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
 import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.WobbleArm;
+import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
 
 public class TeleOpState extends CyberarmState {
 
@@ -12,14 +15,24 @@ public class TeleOpState extends CyberarmState {
     private float rightJoystickDegrees;
     private double rightJoystickMagnitude;
 
-    private double POWER_SPRINT = 0.4;
+    private double[] powers = {0,0,0,0};
-    private double POWER_NORMAL = 0.2;
+    private double drivePower = 1;
-    private double powerScale = 0.2 ;
+    private static final double TURN_POWER = 1;
     private boolean toggleSpeedInput = false;
+    private Launch launchState;
+    private boolean launching;
+    private ProgressRingBelt ringBeltState;
+    private boolean CollectorOn;
+    private boolean xPrev;
+    private boolean yPrev;
+    private boolean aPrev;
+    private boolean bPrev;
+    private boolean rbPrev;
+    private boolean wobbleArmUp = true;
+    private boolean wobbleGrabOpen = false;
 
 
     private boolean launchInput = false;
-    private CyberarmState launchDriveState = new DriveToCoordinates(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y, robot.LAUNCH_ROTATION, 10, POWER_NORMAL, 50);
 
 
     public TeleOpState(Robot robot) {
@@ -47,46 +60,117 @@ public class TeleOpState extends CyberarmState {
         rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
         rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
 
+        if (leftJoystickMagnitude > 0.66) {
+            drivePower = 1 ;
+        } else {
+            drivePower = 0.6;
+        }
+
         double[] powers = {0,0,0,0};
 
-        if (engine.gamepad1.y) {
+        boolean y = engine.gamepad1.y;
-            double distanceToTarget = Math.hypot(robot.LAUNCH_POSITION_X - robot.getLocationX(), robot.LAUNCH_POSITION_Y - robot.getLocationY());
+        if (y) {
-            if (distanceToTarget > robot.LAUNCH_TOLERANCE_POS) {
-                powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y), POWER_NORMAL, robot.LAUNCH_ROTATION);
 
-            } else if (robot.getRelativeAngle(robot.LAUNCH_ROTATION, robot.getRotation()) > robot.LAUNCH_TOLERANCE_FACE) {
+            //Launch Sequence
-                //todo add launch sequence
+
+            double distanceToTarget = Math.hypot(Robot.LAUNCH_POSITION_X - robot.getLocationX(), Robot.LAUNCH_POSITION_Y - robot.getLocationY());
+            if (distanceToTarget > Robot.LAUNCH_TOLERANCE_POS) {
+                powers = robot.getMecanumPowers(robot.getAngleToPosition(Robot.LAUNCH_POSITION_X, Robot.LAUNCH_POSITION_Y), drivePower, Robot.LAUNCH_ROTATION);
+
+            } else if (robot.getRelativeAngle(Robot.LAUNCH_ROTATION, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) {
+
+                launchState = new Launch(robot);
+                addParallelState(launchState);
+
+            } else {
+
+                double[] facePowers =  robot.getFacePowers(Robot.LAUNCH_ROTATION, TURN_POWER);
+                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
 
             }
-        } else {
-            boolean joystickButton = engine.gamepad1.left_stick_button;
 
-            if (joystickButton && !toggleSpeedInput) {
+        }
-                if (powerScale == POWER_NORMAL) {
-                    powerScale = POWER_SPRINT;
-                } else {
-                    powerScale = POWER_NORMAL;
-                }
-            }
 
-            toggleSpeedInput = joystickButton;
+        if (!y || ( launchState != null && launchState.getHasFinished())) {
 
+            //Normal Driver Controls
 
             if (rightJoystickMagnitude == 0) {
                 if (leftJoystickMagnitude !=0) {
-                    powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees);
+                    powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, leftJoystickDegrees);
                 }
             } else {
                 if (leftJoystickMagnitude == 0) {
-                    double[] facePowers =  robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude);
+                    double[] facePowers =  robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude);
                     powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
                 } else {
-                    powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees);
+                    powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, rightJoystickDegrees);
                 }
             }
         }
 
         robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
 
+        this.powers = powers;
+
+
+        boolean x = engine.gamepad2.x;
+        if (x && !xPrev && childrenHaveFinished()) {
+            if (CollectorOn) {
+                robot.collectionMotor.setPower(0);
+                CollectorOn = false;
+            } else {
+                robot.collectionMotor.setPower(1);
+                CollectorOn = true;
+            }
+        }
+        xPrev = x;
+
+        boolean y2 = engine.gamepad2.y;
+        if (y2 && !yPrev && childrenHaveFinished()) {
+            launchState = new Launch(robot);
+            addParallelState(launchState);
+        }
+        yPrev = y2;
+
+        boolean a = engine.gamepad2.a;
+        if (a && !aPrev && childrenHaveFinished()) {
+            addParallelState(new ProgressRingBelt(robot));
+        }
+        aPrev = a;
+
+        boolean b = engine.gamepad2.b;
+        if (b && !bPrev) {
+            wobbleArmUp = !wobbleArmUp;
+            addParallelState(new WobbleArm(robot, wobbleArmUp, 100));
+        }
+        bPrev = b;
+
+        boolean rb = engine.gamepad2.right_bumper;
+        if (rb && !rbPrev) {
+            wobbleGrabOpen = !wobbleGrabOpen ;
+            addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100));
+        }
+        rbPrev = rb;
     }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Powers");
+        for (double power : powers) {
+            engine.telemetry.addData(" ", power);
+        }
+
+
+        engine.telemetry.addLine("Location");
+        engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()))+","+round(robot.ticksToInches(robot.getLocationY()))+")");
+        engine.telemetry.addData("Rotation ", robot.getRotation());
+        engine.telemetry.addData("totalV", robot.totalV);
+    }
+
+    private float round(double number) {
+        return ((float) Math.floor(number*100)) / 100;
+    }
+
+
 }

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

@@ -0,0 +1,48 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class WobbleArm extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private boolean armUp;
+    private long waitTime = -1;
+
+    public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+        this.armUp = armUp;
+    }
+
+    public WobbleArm(Robot robot, boolean armUp, long waitTime) {
+        this.robot = robot;
+        this.armUp = armUp;
+        this.waitTime = waitTime;
+    }
+
+    @Override
+    public void init() {
+        if (waitTime == -1) {
+            waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
+        }
+    }
+
+    @Override
+    public void start() {
+        robot.wobbleArmMotor.setPower(0.3);
+        if (armUp) {
+            robot.wobbleArmMotor.setTargetPosition(0);
+        } else {
+            robot.wobbleArmMotor.setTargetPosition(Robot.WOBBLE_ARM_DOWN);
+        }
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(runTime() > waitTime);
+    }
+
+}

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

@@ -0,0 +1,47 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class WobbleGrab extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private boolean open;
+    private long waitTime = -1;
+
+    public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+        this.open = open;
+    }
+
+    public WobbleGrab(Robot robot, boolean armUp, long waitTime) {
+        this.robot = robot;
+        this.open = armUp;
+        this.waitTime = waitTime;
+    }
+
+    @Override
+    public void init() {
+        if (waitTime == -1) {
+            waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
+        }
+    }
+
+    @Override
+    public void start() {
+        if (open) {
+            robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+        } else {
+            robot.wobbleGrabServo.setPosition(0);
+        }
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(runTime() > waitTime);
+    }
+
+}

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

@@ -1,39 +0,0 @@
-package org.timecrafters.UltimateGoal.HardwareTesting;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Competition.Robot;
-
-
-public class FullTest extends CyberarmState {
-
-    private Robot robot;
-
-    public FullTest(Robot robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void init() {
-
-    }
-
-    @Override
-    public void exec() {
-        double p = 0;
-
-        if (engine.gamepad1.x) {
-            p = 0.1;
-        }
-
-
-        robot.setDrivePower(p,p,p,p);
-        robot.collectionMotor.setPower(0.5);
-        robot.ringBeltMotor.setPower(p);
-        robot.launchMotor.setPower(p);
-    }
-
-    @Override
-    public void telemetry() {
-
-    }
-}

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

@@ -49,7 +49,7 @@ public class MecanumFunctionTest extends CyberarmState {
     @Override
     public void exec() {
         robot.updateLocation();
-        robot.record();
+//        robot.record();/////////////
 
         AngularVelocity angularVelocity = robot.imu.getAngularVelocity();
 
@@ -129,9 +129,9 @@ public class MecanumFunctionTest extends CyberarmState {
         engine.telemetry.addData("Z", aVz);
 
         engine.telemetry.addLine("Powers");
-        engine.telemetry.addData("FL", robot.driveFrontLeft.motor.getPower());
+        engine.telemetry.addData("FL", robot.driveFrontLeft.getPower());
-        engine.telemetry.addData("FR", robot.driveFrontRight.motor.getPower());
+        engine.telemetry.addData("FR", robot.driveFrontRight.getPower());
-        engine.telemetry.addData("BL", robot.driveBackLeft.motor.getPower());
+        engine.telemetry.addData("BL", robot.driveBackLeft.getPower());
-        engine.telemetry.addData("BR", robot.driveBackRight.motor.getPower());
+        engine.telemetry.addData("BR", robot.driveBackRight.getPower());
     }
 }

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

@@ -0,0 +1,36 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+
+public class ServoPosTest extends CyberarmState {
+
+    private Servo servo;
+    private double servoPos;
+
+    public ServoPosTest() {
+    }
+
+    @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;
+        servo.setPosition(servoPos);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position", servoPos);
+    }
+}

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

@@ -8,19 +8,19 @@ import org.timecrafters.UltimateGoal.Competition.Robot;
 @TeleOp (name = "Hardware test", group = "test")
 public class TestingEngine extends CyberarmEngine {
 
-    private Robot robot;
+//    private Robot robot;
-    @Override
+//    @Override
-    public void init() {
+//    public void init() {
-        robot = new Robot(hardwareMap);
+//        robot = new Robot(hardwareMap);
-        robot.initHardware();
+//        robot.initHardware();
-        super.init();
+//        super.init();
-    }
+//    }
 
     @Override
     public void setup() {
-        addState(new FullTest(robot));
+        addState(new ServoPosTest());
     }
-//
+
 //    @Override
 //    public void stop() {
 //        robot.saveRecording();

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

@@ -0,0 +1,30 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+
+public class WobbleArmTest extends CyberarmState {
+
+    private int currentPos;
+    private DcMotor motor;
+
+    @Override
+    public void init() {
+        motor = engine.hardwareMap.dcMotor.get("wobbleArm");
+        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        currentPos = motor.getCurrentPosition();
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position", currentPos);
+    }
+}