Some Small preparations for Java Class && the County Fair Demo stuff

.idea/compiler.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
   <component name="CompilerConfiguration">
-    <bytecodeTargetLevel target="11" />
+    <bytecodeTargetLevel target="1.8" />
   </component>
 </project>

.idea/misc.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="1.8" project-jdk-type="JavaSDK">
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" default="true" project-jdk-name="1.8" project-jdk-type="JavaSDK">
     <output url="file://$PROJECT_DIR$/build/classes" />
   </component>
   <component name="ProjectType">

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

@@ -150,7 +150,7 @@ public abstract class CyberarmEngine extends OpMode {
    * Recursively stop states
    * @param state State to stop
    */
-  private void stopState(CyberarmState state) {
+  public void stopState(CyberarmState state) {
     state.setHasFinished(true);
     state.stop();
 

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

@@ -1,10 +1,11 @@
 package org.timecrafters.UltimateGoal.Calibration;
 
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.UltimateGoal.Competition.Robot;
-
+@Disabled
 @TeleOp (name = "Calibration", group = "test")
 public class CalibrationEngine extends CyberarmEngine {
     private Robot robot;

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

@@ -35,7 +35,7 @@ public class AutoEngine extends CyberarmEngine {
         // since we've preloaded three rings, the ring belt stage is set to account for this;
         robot.ringBeltStage = 3;
 
-        //Autonomous specific Variables
+        //Autonomous specific Configuration Variables
         float rotation = robot.stateConfiguration.variable("system", "startPos", "direction").value();
         double locationX = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "x").value());
         double locationY = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "y").value());

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

@@ -1,5 +1,9 @@
 package org.timecrafters.UltimateGoal.Competition.Autonomous;
 
+/*
+The FindWobbleGoal state is used in teleOp and Autonomous to aid in capturing the wobble goal.
+*/
+
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.timecrafters.UltimateGoal.Competition.Robot;
@@ -47,6 +51,7 @@ public class FindWobbleGoal extends CyberarmState {
         robot.updateLocation();
         double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
 
+        //Stage 1: scan back and forth untile the sensor is in line with the wobble goal.
         if (sensorValue > turnCheck) {
 
             float rotation = robot.getRelativeAngle(startRotation,robot.getRotation());
@@ -65,18 +70,23 @@ public class FindWobbleGoal extends CyberarmState {
             ccCheckPrev = ccCheck;
 
         } else {
+            //Stage 2: drive toward wobble goal until it's close enough to grab
              if (sensorValue > driveCheck) {
                  if (!foundGoalRotation) {
                      foundGoalRotation = true;
                      wobbleGoalRotation = robot.getRotation();
                  }
-                 double[] powers = robot.getMecanumPowers(wobbleGoalRotation - 90, power*2 , wobbleGoalRotation);
+                 double[] powers = robot.getMecanumPowers(
+                         wobbleGoalRotation - 90,
+                         power*2 , wobbleGoalRotation);
                  robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
              } else {
+                 //stage 3: grab the wobble goal && finish the state
                  endSearch();
              }
         }
 
+        //if the search takes too long, the robot grabs and finishes the state
         if (runTime() > timeLimit) {
             endSearch();
         }

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

@@ -1,39 +0,0 @@
-package org.timecrafters.UltimateGoal.Competition.Demo;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Competition.Robot;
-import org.timecrafters.UltimateGoal.Competition.TeleOp.Player1;
-import org.timecrafters.UltimateGoal.Competition.TeleOp.Player2;
-
-public class DemoControl extends CyberarmState {
-
-    private Robot robot;
-
-    public DemoControl(Robot robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void start() {
-//        addParallelState(new Demo1(robot));
-//        addParallelState(new Demo2(robot));
-    }
-
-    @Override
-    public void exec() {
-
-    }
-
-    @Override
-    public void telemetry() {
-
-        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());
-    }
-
-    private float round(double number,double unit) {
-        return (float) (Math.floor(number/unit) * unit);
-    }
-
-}

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

@@ -1,14 +1,12 @@
 package org.timecrafters.UltimateGoal.Competition.Demo;
 
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 import org.timecrafters.UltimateGoal.Competition.TeleOp.TeleOpState;
 
-@Disabled
+@TeleOp (name = "Demo: Game",group = "demo")
-@TeleOp (name = "Demo")
 public class DemoEngine extends CyberarmEngine {
 
     private Robot robot;
@@ -17,14 +15,14 @@ public class DemoEngine extends CyberarmEngine {
     public void init() {
         robot = new Robot(hardwareMap);
         robot.initHardware();
-        robot.wobbleGrabServo.setPosition(0);
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
         robot.webCamServo.setPosition(0);
         super.init();
     }
 
     @Override
     public void setup() {
-        addState(new DemoControl(robot));
+        addState(new DemoState(robot));
     }
 
     @Override

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

@@ -0,0 +1,33 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+@TeleOp (name = "Demo: Tank",group = "demo")
+public class DemoEngineTank extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        robot.webCamServo.setPosition(0);
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new DemoStateTank(robot));
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+        robot.saveRecording();
+        super.stop();
+    }
+}

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

@@ -0,0 +1,194 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+/*
+The Player1 state has all the controls for player one. The Player One and Player Two controls are
+kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
+from running at the same time that shouldn't be.
+*/
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl;
+
+public class DemoState extends CyberarmState {
+    private Robot robot;
+
+    //normal drive control
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double rightJoystickMagnitudePrevious;
+
+    private double faceControlThreshold;
+    private float cardinalSnapping;
+    private float pairSnapping;
+
+    private float faceDirection = 0;
+    private double[] powers = {0,0,0,0};
+    private double drivePower = 1;
+    private boolean lbPrev;
+
+    private double refinePower;
+
+    public DemoState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        cardinalSnapping = robot.stateConfiguration.variable(
+                "tele","control", "cardinalSnapping").value();
+        pairSnapping = robot.stateConfiguration.variable(
+                "tele","control", "pairSnapping").value();
+        faceControlThreshold = robot.stateConfiguration.variable(
+                "tele","control", "faceControlT").value();
+        refinePower = robot.stateConfiguration.variable(
+                "tele","control", "refPower").value();
+
+    }
+
+    @Override
+    public void start() {
+        faceDirection = robot.getRotation();
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        Gamepad gamepad = engine.gamepad1;
+        if (engine.gamepad2.right_trigger != 0) {
+            gamepad = engine.gamepad2;
+        }
+
+        //toggle for drive speed
+        boolean lb = engine.gamepad2.left_stick_button;
+        if (lb && !lbPrev) {
+            if (drivePower == 1) {
+                drivePower = 0.5;
+            } else {
+                drivePower = 1;
+            }
+        }
+        lbPrev = lb;
+
+        //Calculate Joystick Vector
+        double leftJoystickX = gamepad.left_stick_x;
+        double leftJoystickY = gamepad.left_stick_y;
+
+        leftJoystickDegrees = robot.getRelativeAngle(0,
+                (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
+        leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+        double rightJoystickX = gamepad.right_stick_x;
+        double rightJoystickY = gamepad.right_stick_y;
+
+        rightJoystickDegrees = robot.getRelativeAngle(0,
+                (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
+        rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+        //allows the the driver to indicate which direction the robot is currently looking
+        //so that the controller and robot can be re-synced in the event of a bad initial
+        //rotation.
+        if (gamepad.back) {
+            float newRotation = 0;
+
+            if (rightJoystickMagnitude != 0) {
+                newRotation = rightJoystickDegrees;
+            }
+
+            robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY());
+            faceDirection = newRotation;
+        }
+
+        //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;
+
+        //The D-pad is used if the drivers need to get a more precise angle than they can get
+        //using the face joystick
+        if (gamepad.dpad_right) {
+            powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
+            faceDirection = robot.getRotation();
+        } else if (gamepad.dpad_left) {
+            powers = new double[]{-refinePower, refinePower, -refinePower, refinePower};
+            faceDirection = robot.getRotation();
+        } else if (leftJoystickMagnitude == 0) {
+            double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
+            powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
+        } else {
+            //drives the robot in the direction of the move joystick while facing the direction
+            //of the look joystick. if the move direction is almost aligned/perpendicular to the
+            //look joystick,
+            powers = robot.getMecanumPowers(
+                    snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection),
+                    drivePower, faceDirection);
+        }
+
+        robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
+
+
+        //LED feedback control
+        double ringBeltPower = robot.ringBeltMotor.getPower();
+        if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() -
+                robot.ringBeltMotor.getCurrentPosition()) > 10) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );
+        } else if (ringBeltPower < 0) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);
+        }  else {
+            if (drivePower == 1) {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
+            } else {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Controler Directions");
+        engine.telemetry.addData("Right", rightJoystickDegrees);
+        engine.telemetry.addData("Left", leftJoystickDegrees);
+
+        engine.telemetry.addData("face", faceDirection);
+
+        engine.telemetry.addData("Player 1 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+    //This just checks if the wobble arm runmode is already the desired mode before setting it.
+    //I don't know if this is actually helpful
+    private void setArmMode(DcMotor.RunMode runMode) {
+        if (robot.wobbleArmMotor.getMode() != runMode) {
+            robot.wobbleArmMotor.setMode(runMode);
+        }
+    }
+
+    private float snapToCardinal(float angle, float snapping, float offset) {
+        int o = (int) offset + 180;
+        o %= 90;
+        for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) {
+            if (angle >= cardinal-snapping && angle <= cardinal+snapping) {
+                angle = cardinal;
+            }
+        }
+        return angle;
+    }
+}

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

@@ -0,0 +1,55 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+/*
+The Player1 state has all the controls for player one. The Player One and Player Two controls are
+kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
+from running at the same time that shouldn't be.
+*/
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl;
+
+public class DemoStateTank extends CyberarmState {
+    private Robot robot;
+
+    private double drivePower = 1;
+    private boolean lbPrev;
+
+
+    public DemoStateTank(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        Gamepad gamepad = engine.gamepad1;
+        if (engine.gamepad2.right_bumper) {
+            gamepad = engine.gamepad2;
+        }
+
+        //toggle for drive speed
+        boolean lb = engine.gamepad2.left_stick_button;
+        if (lb && !lbPrev) {
+            if (drivePower == 1) {
+                drivePower = 0.5;
+            } else {
+                drivePower = 1;
+            }
+        }
+        lbPrev = lb;
+
+        double left = -drivePower * gamepad.left_stick_y;
+        double right = -drivePower * gamepad.right_stick_y;
+        robot.setDrivePower(left,right,left,right);
+
+    }
+
+}

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

@@ -1,6 +1,10 @@
 package org.timecrafters.UltimateGoal.Competition;
 
-import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+/*
+The Launch state is used in teleOp and Autonomous. In addition to launching any rings by cycling the
+ring belt, this state returns the ring belt to the starting position
+*/
+
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -45,10 +49,7 @@ public class Launch extends CyberarmState {
 
     @Override
     public void exec() {
-        //detect when limit switch is initially triggered
+        //jam counter measures
-        boolean detectingPass = robot.limitSwitch.isPressed();
-        int beltPos = robot.ringBeltMotor.getCurrentPosition();
-
         if (robot.beltIsStuck() && childrenHaveFinished()) {
             long currentTime = System.currentTimeMillis();
             if (stuckStartTime == 0) {
@@ -60,6 +61,9 @@ public class Launch extends CyberarmState {
             stuckStartTime = 0;
         }
 
+        //detect when limit switch is initially triggered
+        boolean detectingPass = robot.limitSwitch.isPressed();
+        int beltPos = robot.ringBeltMotor.getCurrentPosition();
         if (detectingPass && !detectedPass) {
             //finish once the ring belt has cycled all the way through and then returned to
             //the first receiving position.

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

@@ -16,7 +16,7 @@ public class Pause extends CyberarmState {
         this.actionName = actionName;
     }
 
-    public Pause(Robot robot, boolean open, long waitTime) {
+    public Pause(Robot robot,  long waitTime) {
         this.robot = robot;
         this.waitTime = waitTime;
     }

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

@@ -1,10 +1,11 @@
 package org.timecrafters.UltimateGoal.Competition.PreInit;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.UltimateGoal.Competition.Robot;
-
+@Disabled
 @Autonomous (name = "Load Rings")
 public class PreInitEngine extends CyberarmEngine {
 

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

@@ -1,8 +1,11 @@
 package org.timecrafters.UltimateGoal.Competition;
 
+/*
+The ProgressRingBelt state is used in teleOp to automatically move the ring belt.
+*/
+
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.robotcore.hardware.DcMotor;
-
 import org.cyberarm.engine.V2.CyberarmState;
 
 public class ProgressRingBelt extends CyberarmState {
@@ -27,13 +30,20 @@ public class ProgressRingBelt extends CyberarmState {
     @Override
     public void start() {
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
+
+        //The first two progressions should move to preprare for another ring.
         if (robot.ringBeltStage < 2) {
             targetPos = currentPos + robot.ringBeltGap;
             prep();
+
+            //The third progression needs to only move forward enought to block the ring from
+            //falling out
         } else if (robot.ringBeltStage == 2) {
             targetPos = currentPos + 150;
             prep();
             prepLaunch = !robot.initLauncher;
+
+            //If the ring belt is already full, It does  allow another progression
         } else if (robot.ringBeltStage > 2) {
             setHasFinished(true);
         }
@@ -44,6 +54,7 @@ public class ProgressRingBelt extends CyberarmState {
     @Override
     public void exec() {
 
+        //finished state when the target position is reached
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         if (currentPos >= targetPos) {
             if(prepLaunch) {
@@ -53,6 +64,7 @@ public class ProgressRingBelt extends CyberarmState {
             setHasFinished(true);
         }
 
+        //belt jam mitigation
         if (robot.beltIsStuck() && childrenHaveFinished()) {
             long currentTime = System.currentTimeMillis();
             if (stuckStartTime == 0) {

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

@@ -1,4 +1,4 @@
-package org.timecrafters.UltimateGoal.Competition;
+ package org.timecrafters.UltimateGoal.Competition;
 
 /*
 The robot object contains all the hardware and functions that are used in both teleOp and
@@ -14,6 +14,7 @@ import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.hardware.rev.RevTouchSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
@@ -53,7 +54,8 @@ public class Robot {
     }
 
     //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit
-    //variables saved on the phone, without having to re-download the whole program.
+    //variables saved on the phone, without having to re-download the whole program. This is
+    //especially useful for autonomous route tuning
     public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
 
     //We use the IMU to get reliable rotation and angular velocity information. Experimentation has
@@ -74,6 +76,12 @@ public class Robot {
     public DcMotor encoderBack;
 
     //Motion Constants
+
+    //related graphs
+    //https://www.desmos.com/calculator/gndnkjndu9
+    //https://www.desmos.com/calculator/w0rebnftvg
+    //https://www.desmos.com/calculator/qxa1rq8hrv
+
     static final double CUBIC_CORRECTION = 0.035;
     static final double FACE_CUBIC_CORRECTION = 0.025;
     static final double LINEAR_CORRECTION = 0.055;
@@ -81,10 +89,12 @@ public class Robot {
     static final double FACE_LINEAR_CORRECTION = 0.025;
     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));
+    static final double MOMENTUM_HORIZONTAL_CORRECTION =
+            -(Math.log10(MOMENTUM_MAX_CORRECTION-1) / Math.log10(MOMENTUM_CORRECTION));
     static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1;
     static final double FACE_MOMENTUM_CORRECTION = 1.06;
-    static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1)/Math.log10(FACE_MOMENTUM_CORRECTION));
+    static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION =
+            -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1) / Math.log10(FACE_MOMENTUM_CORRECTION));
     static final double ZERO_POWER_THRESHOLD = 0.25;
 
     //Unit Conversion Constants
@@ -107,7 +117,7 @@ public class Robot {
     private float rotationPrevious = 0;
     public float angularVelocity;
 
-    //vuforia navigation
+    //vuforia && tensorFlow Stuff
     private WebcamName webcam;
     private VuforiaLocalizer vuforia;
 
@@ -118,8 +128,10 @@ public class Robot {
     // Inches Left of axis of rotation
     static final float CAMERA_LEFT_DISPLACEMENT = 4f;
 
-    static final double CAMERA_DISPLACEMENT_MAG = Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
+    static final double CAMERA_DISPLACEMENT_MAG =
-    static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
+            Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
+    static final float CAMERA_DISPLACEMENT_DIRECTION =
+            (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
 
     public boolean trackableVisible;
     private VuforiaTrackables targetsUltimateGoal;
@@ -145,6 +157,7 @@ public class Robot {
     public static final double LAUNCH_POWER = 0.715;
 
     private static final long LAUNCH_ACCEL_TIME = 500;
+    //These variables were originally going to be used in both autonomous and teleOp
     public double launchPositionX;
     public double launchPositionY;
     public float launchRotation;
@@ -182,22 +195,17 @@ public class Robot {
 
     //Debugging
     public double totalV;
-    public double visionX;
+    private String TestingRecord = "Raw IMU, Delta, Saved";
-    public double visionY;
-    public double visionZ;
-    public float rawAngle;
-    private String TestingRecord = "x,y";
-
     public double forwardVector;
     public double sidewaysVector;
-
     public double traveledForward = 0;
-    public double traveledRight;
+    public DcMotorEx motorAmpsTest;
 
+    //All our hardware initialization in one place, for everything that is the same in TeleOp and
+    //Autonomous
     public void initHardware() {
 
-        limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
+        //drive motors
-
         driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
         driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
         driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
@@ -226,9 +234,12 @@ public class Robot {
         wobbleArmMotor.setTargetPosition(0);
         wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
-        wobbleUpPos = stateConfiguration.variable("system","arm", "up").value();
+        wobbleUpPos = stateConfiguration.variable(
-        wobbleDownPos = stateConfiguration.variable("system","arm", "down").value();
+                "system","arm", "up").value();
-        wobbleDropPos = stateConfiguration.variable("system","arm", "drop").value();
+        wobbleDownPos = stateConfiguration.variable(
+                "system","arm", "down").value();
+        wobbleDropPos = stateConfiguration.variable(
+                "system","arm", "drop").value();
 
         wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
 
@@ -236,7 +247,7 @@ public class Robot {
         wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
 
 
-        //init ring belt
+        //init collection motor
         collectionMotor = hardwareMap.dcMotor.get("collect");
         collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
 
@@ -244,10 +255,17 @@ public class Robot {
         ringBeltMotor = hardwareMap.dcMotor.get("belt");
         ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        beltMaxStopTime = stateConfiguration.variable("system","belt", "maxStopTime").value();
+
-        beltMaxStopTicks = stateConfiguration.variable("system","belt", "maxStopTicks").value();
+        limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
-        beltReverseTicks = stateConfiguration.variable("system","belt", "reverseTicks").value();
+
-        ringBeltGap = stateConfiguration.variable("system","belt","gap").value();
+        beltMaxStopTime = stateConfiguration.variable(
+                "system","belt", "maxStopTime").value();
+        beltMaxStopTicks = stateConfiguration.variable(
+                "system","belt", "maxStopTicks").value();
+        beltReverseTicks = stateConfiguration.variable(
+                "system","belt", "reverseTicks").value();
+        ringBeltGap = stateConfiguration.variable(
+                "system","belt","gap").value();
 
         //init IMU
         imu  = hardwareMap.get(BNO055IMU.class, "imu");
@@ -279,9 +297,12 @@ public class Robot {
         webCamServo = hardwareMap.servo.get("look");
         webCamServo.setDirection(Servo.Direction.REVERSE );
 
-        minCheckVelocity =stateConfiguration.variable("system", "camera", "minCheckV").value();
+        minCheckVelocity =stateConfiguration.variable(
-        vuforiaRotationCull = stateConfiguration.variable("system", "camera", "rCull").value();
+                "system", "camera", "minCheckV").value();
-        minCheckDurationMs =stateConfiguration.variable("system", "camera", "minCheckMS").value();
+        vuforiaRotationCull = stateConfiguration.variable(
+                "system", "camera", "rCull").value();
+        minCheckDurationMs =stateConfiguration.variable(
+                "system", "camera", "minCheckMS").value();
 
         //Init Launch Motor
         DcMotor launcher = hardwareMap.dcMotor.get("launcher");
@@ -296,8 +317,12 @@ public class Robot {
         launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
-        initLauncher = stateConfiguration.action("system","initLauncher").enabled;
+        //This is a debugging option that automatically turns on the launch wheel during init.
-        reduceLaunchPos = stateConfiguration.variable("system", "launchPos", "reducePower").value();
+        //This can be disabled using a variable in the TimeCraftersConfiguration
+        initLauncher = stateConfiguration.action(
+                "system","initLauncher").enabled;
+        reduceLaunchPos = stateConfiguration.variable(
+                "system", "launchPos", "reducePower").value();
 
         if (initLauncher) {
             double launcherPower = 0;
@@ -307,10 +332,13 @@ public class Robot {
                 launchMotor.setPower(launcherPower);
             }
         }
-        //
+
-        launchPositionX = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","x").value());
+        launchPositionX = inchesToTicks((double) stateConfiguration.variable(
-        launchPositionY = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","y").value());
+                "system", "launchPos","x").value());
-        launchRotation = stateConfiguration.variable("system", "launchPos","rot").value();
+        launchPositionY = inchesToTicks((double) stateConfiguration.variable(
+                "system", "launchPos","y").value());
+        launchRotation = stateConfiguration.variable(
+                "system", "launchPos","rot").value();
 
         initTensorFlow();
 
@@ -319,8 +347,10 @@ public class Robot {
 
     private void initVuforia() {
 
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
-        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+                "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        VuforiaLocalizer.Parameters parameters =
+                new VuforiaLocalizer.Parameters(cameraMonitorViewId);
 
         parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
         parameters.cameraName = webcam;
@@ -346,20 +376,27 @@ public class Robot {
 
         redAllianceTarget.setLocation(OpenGLMatrix
                 .translation(0, -halfField, mmTargetHeight)
-                .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180)));
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, XYZ, DEGREES, 90, 0, 180)));
         frontWallTarget.setLocation(OpenGLMatrix
                 .translation(-halfField, 0, mmTargetHeight)
-                .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)));
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)));
         redTowerGoalTarget.setLocation(OpenGLMatrix
                 .translation(halfField, -quadField, mmTargetHeight)
-                .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));
 
         OpenGLMatrix robotFromCamera = OpenGLMatrix
-                .translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch, CAMERA_LEFT_DISPLACEMENT * mmPerInch, CAMERA_VERTICAL_DISPLACEMENT * mmPerInch)
+                .translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch,
-                .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, -90, 0, 0));
+                        CAMERA_LEFT_DISPLACEMENT * mmPerInch,
+                        CAMERA_VERTICAL_DISPLACEMENT * mmPerInch)
+                .multiplied(Orientation.getRotationMatrix(
+                        EXTRINSIC, YZX, DEGREES, -90, 0, 0));
 
         for (VuforiaTrackable trackable : trackables) {
-            ((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection);
+            ((VuforiaTrackableDefaultListener) trackable.getListener())
+                    .setPhoneInformation(robotFromCamera, parameters.cameraDirection);
         }
 
         targetsUltimateGoal.activate();
@@ -373,11 +410,15 @@ public class Robot {
         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
                 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
         TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
-        parameters.minResultConfidence = stateConfiguration.variable("system", "camera", "minConfidence").value();
+        parameters.minResultConfidence = stateConfiguration.variable(
+                "system", "camera", "minConfidence").value();
         tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
-        tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
+        tfObjectDetector.loadModelFromAsset(
+                "UltimateGoal.tflite", "Quad", "Single");
     }
 
+    //Localization Function! This function is represented in a flow diagram, earlier in the
+    //software section
     //run this in every exec to track the robot's location.
     public void updateLocation(){
 
@@ -417,13 +458,15 @@ public class Robot {
             ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD;
         }
 
-        forwardVector = ((encoderLeftChange+encoderRightChange)/2) -  (rotationChange* ticksPerDegreeForward);
+        forwardVector = ((encoderLeftChange+encoderRightChange)/2) -
+                (rotationChange* ticksPerDegreeForward);
 
         traveledForward += forwardVector;
         sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
 
         double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
-        double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
+        double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) +
+                Math.atan2(sidewaysVector,forwardVector);
 
         double xChange = magnitude * (Math.sin(direction));
         double yChange = magnitude * (Math.cos(direction));
@@ -434,11 +477,15 @@ public class Robot {
         Robot.rotation += rotationChange;
         Robot.rotation = scaleAngleRange(Robot.rotation);
 
-        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
+        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) +
+                Math.abs(encoderBackChange);
+
+//        record(""+System.currentTimeMillis()+", "+imuAngle);
     }
 
-
+    //Experimentation has demonstrated that Vuforia dosen't provide good data while the camera
-
+    //is moving or rotating. This function detects if conditions are appropriate to run vuforia and
+    //get more accurate results
     public void syncIfStationary() {
         if (totalV < minCheckVelocity) {
             long timeCurrent = System.currentTimeMillis();
@@ -459,7 +506,9 @@ public class Robot {
         trackableVisible = false;
         for (VuforiaTrackable trackable : trackables) {
             if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
-                OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
+                OpenGLMatrix robotLocation =
+                        ((VuforiaTrackableDefaultListener)trackable.getListener())
+                                .getUpdatedRobotLocation();
 
                 //this is used for debugging purposes.
                 trackableVisible = true;
@@ -470,7 +519,8 @@ public class Robot {
 
                 //For our tournament, it makes sense to make zero degrees towards the goal.
                 //Orientation is inverted to have clockwise be positive.
-                Orientation orientation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
+                Orientation orientation =
+                        Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
                 float vuforiaRotation = 90-orientation.thirdAngle;
 
                 if (vuforiaRotation > 180) {
@@ -484,8 +534,10 @@ public class Robot {
                     double camX = -translation.get(1) / mmPerInch;
                     double camY = translation.get(0) / mmPerInch;
 
-                    double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                    double displaceX = CAMERA_DISPLACEMENT_MAG *
-                    double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                            Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                    double displaceY = CAMERA_DISPLACEMENT_MAG *
+                            Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
 
                     locationX = inchesToTicks(camX - displaceX);
                     locationY = inchesToTicks(camY - displaceY);
@@ -508,18 +560,13 @@ public class Robot {
         return Robot.locationY;
     }
 
+    //This is meant to only be used to indicate starting positions and to reorient the robot.
     public void setLocalization(float rotation, double x, double y) {
         Robot.rotation = rotation;
         Robot.locationX = x;
         Robot.locationY = y;
     }
 
-    //Manually set the position of the robot on the field.
-    public void setCurrentPosition(float rotation, double x, double y) {
-        Robot.rotation = rotation;
-        Robot.locationX = x;
-        Robot.locationY = y;
-    }
 
     //returns the angle from the robot's current position to the given target position.
     public float getAngleToPosition (double x, double y) {
@@ -531,7 +578,7 @@ public class Robot {
 
     }
 
-    //Unit conversion
+    //Unit conversions
     public double ticksToInches(double ticks) {
         return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
     }
@@ -541,26 +588,28 @@ public class Robot {
 
     }
 
-    //Returns the angle between two angles, with positive angles indicating that the reference is
+    //Returns the shortest angle between two directions, with positive angles indicating that the
-    //to the right (clockwise) of the current. Negative angles indicate that the reference is to the
+    // reference is to the right (clockwise) of the current. Negative angles indicate that the
-    //left.
+    // reference is to the left.
     public float getRelativeAngle(float reference, float current) {
         return scaleAngleRange(current - reference);
     }
 
     //Drive Functions
-    public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){
+    public void setDrivePower(double powerFrontLeft, double powerFrontRight,
+                              double powerBackLeft, double powerBackRight){
         driveFrontLeft.setPower(powerFrontLeft);
         driveFrontRight.setPower(powerFrontRight);
         driveBackLeft.setPower(powerBackLeft);
         driveBackRight.setPower(powerBackRight);
     }
 
-    //returns an array of the powers necessary to execute the provided motion. "degreesDirectionMotion"
+    //returns an array of the powers necessary to execute the provided motion.
-    //is the angle relative to the field that the robot should drive at. "degreesDirectionFace" is
+    //"degreesDirectionMotion" is the angle relative to the field that the robot should drive at.
-    //the angle the robot should face relative to the field. The order of the output powers is
+    //"degreesDirectionFace" is the angle the robot should face relative to the field. The order of
-    //is ForwardLeft, ForwardRight, BackLeft, BackRight
+    //the output powers is ForwardLeft, ForwardRight, BackLeft, BackRight
-    public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) {
+    public double[] getMecanumPowers(float degreesDirectionMotion, double scalar,
+                                     float degreesDirectionFace) {
         angularVelocity = imu.getAngularVelocity().xRotationRate;
 
         //calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion
@@ -582,12 +631,14 @@ public class Robot {
                         LINEAR_CORRECTION * relativeRotation;
 
         if (relativeRotation != 0) {
-            double momentumRelative =  angularVelocity * (relativeRotation / Math.abs(relativeRotation));
+            double momentumRelative =  angularVelocity *
-            double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+                    (relativeRotation / Math.abs(relativeRotation));
+            double exponential =
+                    Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
             double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
             //reduces concern for  momentum when the angle is far away from target
-            turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) * (1  - momentumCorrection)) / 180 );
+            turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) *
-//            turnCorrection *= momentumCorrection;
+                    (1  - momentumCorrection)) / 180 );
         }
 
         double powerForwardRight = scalar * (q + turnCorrection);
@@ -660,6 +711,7 @@ public class Robot {
         return powers;
     }
 
+    //Used to for automated jam countermeasures
     public boolean beltIsStuck() {
         int ringBeltPos = ringBeltMotor.getCurrentPosition();
         boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks);
@@ -677,6 +729,7 @@ public class Robot {
         return angle;
     }
 
+    //Debugging tools
     public void record(String record) {
         TestingRecord+="\n"+record;
     }

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

@@ -17,7 +17,6 @@ public class LaunchControl extends CyberarmState {
 
     @Override
     public void start() {
-        robot.launchMotor.setPower(Robot.LAUNCH_POWER);
 
         if (robot.ringBeltStage > 2) {
             if (robot.ringBeltStage > 4) {

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

@@ -1,5 +1,11 @@
 package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
+/*
+The Player1 state has all the controls for player one. The Player One and Player Two controls are
+kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
+from running at the same time that shouldn't be.
+*/
+
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -40,6 +46,7 @@ public class Player1 extends CyberarmState {
     private double endGameX;
     private double endGameY;
     private float endGameRot;
+    private int loopCount;
 
     private double refinePower;
 
@@ -49,14 +56,21 @@ public class Player1 extends CyberarmState {
 
     @Override
     public void init() {
-        cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
+        cardinalSnapping = robot.stateConfiguration.variable(
-        pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
+                "tele","control", "cardinalSnapping").value();
-        faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
+        pairSnapping = robot.stateConfiguration.variable(
-        refinePower = robot.stateConfiguration.variable("tele","control", "refPower").value();
+                "tele","control", "pairSnapping").value();
+        faceControlThreshold = robot.stateConfiguration.variable(
+                "tele","control", "faceControlT").value();
+        refinePower = robot.stateConfiguration.variable(
+                "tele","control", "refPower").value();
 
-        endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
+        endGameX = robot.stateConfiguration.variable(
-        endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
+                "tele","_endGameStart","x").value();
-        endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
+        endGameY = robot.stateConfiguration.variable(
+                "tele","_endGameStart","y").value();
+        endGameRot = robot.stateConfiguration.variable(
+                "tele","_endGameStart", "r").value();
     }
 
     @Override
@@ -66,8 +80,11 @@ public class Player1 extends CyberarmState {
 
     @Override
     public void exec() {
+        loopCount+=1;
+        robot.record(""+runTime()+", "+loopCount+", "+robot.getRotation());
         robot.updateLocation();
 
+        //toggle for drive speed
         boolean lb = engine.gamepad1.left_stick_button;
         if (lb && !lbPrev) {
             if (drivePower == 1) {
@@ -78,7 +95,9 @@ public class Player1 extends CyberarmState {
         }
         lbPrev = lb;
 
-
+        //This Runs the FindWobbleGoal state as long as the button is pressed. When it's released,
+        //the state is interrupted. If the state finishes while the button is still pressed,
+        //it waits until the button is released before running the state again
         runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
 
         boolean findWobbleInput = engine.gamepad1.x;
@@ -89,7 +108,8 @@ public class Player1 extends CyberarmState {
                 faceDirection = robot.getRotation();
 
                 if (runNextFindWobble && !findWobbleInputPrev) {
-                    findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
+                    findWobbleGoal =
+                            new FindWobbleGoal(robot, "auto", "08_0");
                     addParallelState(findWobbleGoal);
                 }
                 //if the claw is closed, open the claw.
@@ -113,6 +133,8 @@ public class Player1 extends CyberarmState {
         }
         aPrev = a;
 
+        //This logic works the same as the stuff above, accept instead of autonomous wobble grab,
+        //it
         runNextDriveToLaunch = (powerShots == null || powerShots.getHasFinished());
 
         boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput;
@@ -127,29 +149,27 @@ public class Player1 extends CyberarmState {
         }
         driveToLaunchInputPrev = driveToLaunchInput;
 
-//        if (engine.gamepad1.y) {
+        //Normal Driver Controls
-//            robot.setLocalization(endGameRot,endGameX,endGameY);
-//            setHasFinished(true);
-//        }
-
         if (childrenHaveFinished()) {
-            //Normal Driver Controls
+
 
             double leftJoystickX = engine.gamepad1.left_stick_x;
             double leftJoystickY = engine.gamepad1.left_stick_y;
 
-            leftJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
+            leftJoystickDegrees = robot.getRelativeAngle(90,
+                    (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
             leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
 
             double rightJoystickX = engine.gamepad1.right_stick_x;
             double rightJoystickY = engine.gamepad1.right_stick_y;
 
-            rightJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
+            rightJoystickDegrees = robot.getRelativeAngle(90,
+                    (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
             rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
 
             //allows the the driver to indicate which direction the robot is currently looking
             //so that the controller and robot can be re-synced in the event of a bad initial
-            //position.
+            //rotation.
             if (engine.gamepad1.back) {
                 float newRotation = 0;
 
@@ -161,14 +181,19 @@ public class Player1 extends CyberarmState {
                 faceDirection = newRotation;
             }
 
-            //if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
+            //if the driver is letting go of the face joystick, the robot should maintain it's
-            if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
+            //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
+                //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;
 
+            //The D-pad is used if the drivers need to get a more precise angle than they can get
+            //using the face joystick
             if (engine.gamepad1.dpad_right) {
                 powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
                 faceDirection = robot.getRotation();
@@ -182,7 +207,9 @@ public class Player1 extends CyberarmState {
                 //drives the robot in the direction of the move joystick while facing the direction
                 //of the look joystick. if the move direction is almost aligned/perpendicular to the
                 //look joystick,
-                powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), drivePower, faceDirection);
+                powers = robot.getMecanumPowers(
+                        snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection),
+                        drivePower, faceDirection);
             }
 
             robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
@@ -190,7 +217,8 @@ public class Player1 extends CyberarmState {
 
         //LED feedback control
         double ringBeltPower = robot.ringBeltMotor.getPower();
-        if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - robot.ringBeltMotor.getCurrentPosition()) > 10) {
+        if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() -
+                robot.ringBeltMotor.getCurrentPosition()) > 10) {
             robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );
         } else if (ringBeltPower < 0) {
             robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);

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

@@ -1,9 +1,15 @@
 package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
+/*
+The Player2 state has all the controls for player two. This includes a lot of automation and
+emergency features, for if the driver wants to take control unexpectedly
+*/
+
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 import org.timecrafters.UltimateGoal.Competition.Launch;
 import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
 import org.timecrafters.UltimateGoal.Competition.Robot;
@@ -22,6 +28,7 @@ public class Player2 extends CyberarmState {
     private DcMotor.RunMode runModePrev;
     private boolean lbPrev;
     private boolean manualArmHold;
+    private int loops = 0;
 
 
 
@@ -45,9 +52,10 @@ public class Player2 extends CyberarmState {
 
     @Override
     public void exec() {
+//        loops+=1;
+//        robot.record(""+runTime()+", "+loops+", "+robot.getRotation());
 
         //Collector control
-
         double rt = engine.gamepad2.right_trigger;
         double lt = engine.gamepad2.left_trigger;
         if (rt < lt) {
@@ -58,6 +66,8 @@ public class Player2 extends CyberarmState {
             robot.collectionMotor.setPower(0);
         }
 
+        //The childrenHaveFinished() method tracks if there are parallel states that are still
+        //running.
         if (childrenHaveFinished()) {
             //belt progression control
             boolean rb = engine.gamepad2.right_bumper;
@@ -140,33 +150,40 @@ public class Player2 extends CyberarmState {
         }
         lbPrev = lb;
 
-//        if (engine.gamepad1.y) {
+        if (engine.gamepad2.back) {
-//            setHasFinished(true);
+            for (CyberarmState state : children) {
-//        }
+                engine.stopState(state);
+            }
+        }
+
     }
 
+
+    //This just checks if the wobble arm runmode is already the desired mode before setting it.
+    //I don't know if this is actually helpful
     private void setArmMode(DcMotor.RunMode runMode) {
         if (robot.wobbleArmMotor.getMode() != runMode) {
              robot.wobbleArmMotor.setMode(runMode);
         }
     }
 
+
     @Override
     public void telemetry() {
 //        engine.telemetry.addLine("belt");
 //        engine.telemetry.addData("power", robot.ringBeltMotor.getPower());
 //        engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
 //        engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
-
+//
 //        engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
 //        engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed());
 //        engine.telemetry.addData("  Sensor value", robot.wobbleTouchSensor.getValue());
-//        engine.telemetry.addData("Player 2 children", childrenHaveFinished());
+        engine.telemetry.addData("Player 2 children", childrenHaveFinished());
-//        for (CyberarmState state : children) {
+        for (CyberarmState state : children) {
-//            if (!state.getHasFinished()) {
+            if (!state.getHasFinished()) {
-//                engine.telemetry.addLine("" + state.getClass());
+                engine.telemetry.addLine("" + state.getClass());
-//            }
+            }
-//        }
+        }
     }
 
 }

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

@@ -1,12 +1,13 @@
 package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
 import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
 import org.timecrafters.UltimateGoal.Competition.Robot;
-
+@Disabled
 @TeleOp (name = "TeleOp",group = "comp")
 public class TeleOpEngine extends CyberarmEngine {
 

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

@@ -3,6 +3,7 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp;
 import org.cyberarm.engine.V2.CyberarmState;
 import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
 import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
+import org.timecrafters.UltimateGoal.Competition.Pause;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 
 import java.util.ArrayList;
@@ -15,6 +16,8 @@ public class powerShotsControl extends CyberarmState {
     private double endGameY;
     private float endGameRot;
 
+    private double endGamePower;
+
     private int nextState = 0;
 
     private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
@@ -28,11 +31,13 @@ public class powerShotsControl extends CyberarmState {
         endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
         endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
         endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
+        endGamePower = robot.stateConfiguration.variable("tele","_endGameStart", "power").value();
 
         endGameX = robot.inchesToTicks(endGameX);
         endGameY = robot.inchesToTicks(endGameY);
         endGameRot = (float) robot.inchesToTicks(endGameRot);
 
+        states.add(new Pause(robot, "tele","_endGameStart"));
         states.add(new DriveToCoordinates(robot, "tele", "_pow1"));
         states.add(new Face(robot,"tele","_faceZero"));
         states.add(new LaunchControl(robot));
@@ -47,7 +52,7 @@ public class powerShotsControl extends CyberarmState {
     @Override
     public void start() {
         robot.setLocalization(endGameRot,endGameX,endGameY);
-        robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+        robot.launchMotor.setPower(endGamePower);
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/javaClass/aubrey/AubreyFirstState.java

@@ -0,0 +1,22 @@
+package org.timecrafters.javaClass.aubrey;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.javaClass.samples.SampleRobot;
+
+public class AubreyFirstState extends CyberarmState {
+
+    //here, you'll find some of your variables. you can add more as you need them.
+    private SampleRobot robot;
+
+    //This is the constructor. It lets other code bits run use the code you put here
+    public AubreyFirstState(SampleRobot robot) {
+        this.robot = robot;
+    }
+
+    //This is a method. methods are bits of code that can be run elsewhere. 
+    //This one is set up to repeat every few milliseconds
+    @Override
+    public void exec() {
+        
+    }
+}

TeamCode/src/main/java/org/timecrafters/javaClass/samples/AubreyFirstEngine.java

@@ -0,0 +1,26 @@
+package org.timecrafters.javaClass.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.javaClass.aubrey.AubreyFirstState;
+
+@Autonomous(name = "Aubrey: First Program", group = "aubrey")
+public class AubreyFirstEngine extends CyberarmEngine {
+
+    SampleRobot robot;
+
+    @Override
+    public void init() {
+        robot = new SampleRobot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new AubreyFirstState(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobot.java

@@ -0,0 +1,191 @@
+ package org.timecrafters.javaClass.samples;
+
+/*
+The robot object contains all the hardware and functions that are used in both teleOp and
+Autonomous. This includes drive functions, localization functions, shared constants, and a few
+general calculations and debugging tools.
+*/
+
+import android.os.Environment;
+import android.util.Log;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.hardware.rev.RevTouchSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+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;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
+import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+
+import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
+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 SampleRobot {
+
+     private HardwareMap hardwareMap;
+
+     public SampleRobot(HardwareMap hardwareMap) {
+         this.hardwareMap = hardwareMap;
+     }
+
+     //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit
+     //variables saved on the phone, without having to re-download the whole program. This is
+     //especially useful for autonomous route tuning
+     public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
+
+     //We use the IMU to get reliable rotation and angular velocity information. Experimentation has
+     //demonstrated that the accelerometer and related integrations aren't as accurate.
+     public BNO055IMU imu;
+
+     //The LEDs are used to provide driver feedback and for looking beautiful
+     public RevBlinkinLedDriver ledDriver;
+
+     //drive and dead-wheal hardware
+     public DcMotor driveFrontLeft;
+     public DcMotor driveBackLeft;
+     public DcMotor driveFrontRight;
+     public DcMotor driveBackRight;
+
+     public DcMotor encoderLeft;
+     public DcMotor encoderRight;
+     public DcMotor encoderBack;
+
+     //Unit Conversion Constants
+     static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
+     static final int COUNTS_PER_REVOLUTION = 8192;
+     //Launcher
+     public DcMotor launchMotor;
+     public static final double LAUNCH_POWER = 0.715;
+
+     //Ring Intake
+     public DcMotor collectionMotor;
+
+     //Ring Belt
+     public DcMotor ringBeltMotor;
+     public RevTouchSensor limitSwitch;
+
+     //Wobble Goal Arm & Grabber
+     public DcMotor  wobbleArmMotor;
+     public Servo  wobbleGrabServo;
+     public RevColorSensorV3 wobbleColorSensor;
+     public RevTouchSensor wobbleTouchSensor;
+
+     //All our hardware initialization in one place, for everything that is the same in TeleOp and
+     //Autonomous
+     public void initHardware() {
+
+         //drive motors
+         driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
+         driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
+         driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
+         driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
+
+         driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+         driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+         driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+         driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+         //Localization encoders
+         encoderRight = hardwareMap.dcMotor.get("driveFrontRight");
+         encoderBack = hardwareMap.dcMotor.get("driveFrontLeft");
+         encoderLeft = hardwareMap.dcMotor.get("collect");
+
+         encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+         encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+         encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+         //init wobble arm
+         wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm");
+         wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         wobbleArmMotor.setTargetPosition(0);
+         wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+         wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
+
+         wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
+         wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
+
+
+         //init collection motor
+         collectionMotor = hardwareMap.dcMotor.get("collect");
+         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);
+
+         limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
+
+         //init IMU
+         imu  = hardwareMap.get(BNO055IMU.class, "imu");
+
+         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+
+         parameters.mode = BNO055IMU.SensorMode.IMU;
+         parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+         parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+         parameters.loggingEnabled = false;
+
+         imu.initialize(parameters);
+
+         //Init Launch Motor
+         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);
+         launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+         ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds");
+     }
+
+     public float getIMURotation() {
+         return imu.getAngularOrientation().firstAngle;
+     }
+
+     //Unit conversions
+     public double ticksToInches(double ticks) {
+         return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
+     }
+
+     public double inchesToTicks(double inches) {
+         return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE);
+
+     }
+
+
+ }

TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobotEx.java

@@ -0,0 +1,515 @@
+ package org.timecrafters.javaClass.samples;
+
+/*
+The robot object contains all the hardware and functions that are used in both teleOp and
+Autonomous. This includes drive functions, localization functions, shared constants, and a few
+general calculations and debugging tools.
+*/
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.hardware.rev.RevTouchSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+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.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
+import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+import java.util.List;
+
+ public class SampleRobotEx {
+
+     private HardwareMap hardwareMap;
+
+     public SampleRobotEx(HardwareMap hardwareMap) {
+         this.hardwareMap = hardwareMap;
+     }
+
+     //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit
+     //variables saved on the phone, without having to re-download the whole program. This is
+     //especially useful for autonomous route tuning
+     public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
+
+     //We use the IMU to get reliable rotation and angular velocity information. Experimentation has
+     //demonstrated that the accelerometer and related integrations aren't as accurate.
+     public BNO055IMU imu;
+
+     //The LEDs are used to provide driver feedback and for looking beautiful
+     public RevBlinkinLedDriver ledDriver;
+
+     //drive and dead-wheal hardware
+     public DcMotor driveFrontLeft;
+     public DcMotor driveBackLeft;
+     public DcMotor driveFrontRight;
+     public DcMotor driveBackRight;
+
+     public DcMotor encoderLeft;
+     public DcMotor encoderRight;
+     public DcMotor encoderBack;
+
+     //Motion Constants
+     static final double CUBIC_CORRECTION = 0.035;
+     static final double FACE_CUBIC_CORRECTION = 0.025;
+     static final double LINEAR_CORRECTION = 0.055;
+     static final double FACE_MIN_CORRECTION = 0.2;
+     static final double FACE_LINEAR_CORRECTION = 0.025;
+     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));
+     static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1;
+     static final double FACE_MOMENTUM_CORRECTION = 1.06;
+     static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION =
+             -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1) / Math.log10(FACE_MOMENTUM_CORRECTION));
+     static final double ZERO_POWER_THRESHOLD = 0.25;
+
+     //Unit Conversion Constants
+     static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
+     static final int COUNTS_PER_REVOLUTION = 8192;
+     static final float mmPerInch = 25.4f;
+     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;
+
+     //Robot Localization
+     private static double locationX;
+     private static double locationY;
+     private static float rotation;
+
+     private int encoderLeftPrevious = 0;
+     private int encoderBackPrevious = 0;
+     private int encoderRightPrevious = 0;
+     private float rotationPrevious = 0;
+     public float angularVelocity;
+
+     public double forwardVector;
+     public double sidewaysVector;
+
+
+     //vuforia && tensorFlow Stuff
+     private WebcamName webcam;
+     private VuforiaLocalizer vuforia;
+
+     // Inches Forward of axis of rotation
+     static final float CAMERA_FORWARD_DISPLACEMENT  = 8f;
+     // Inches above Ground
+     static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
+     // Inches Left of axis of rotation
+     static final float CAMERA_LEFT_DISPLACEMENT = 4f;
+
+     static final double CAMERA_DISPLACEMENT_MAG =
+             Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
+     static final float CAMERA_DISPLACEMENT_DIRECTION =
+             (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
+
+     public boolean trackableVisible;
+     private VuforiaTrackables targetsUltimateGoal;
+     private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
+     private OpenGLMatrix lastConfirmendLocation;
+
+     private long timeStartZeroVelocity = 0;
+     private long minCheckDurationMs = 500;
+     private int minCheckVelocity = 1;
+     private float vuforiaRotationCull;
+
+     //The servo mount for our camera allows us to look down for ideal TensorFlow and look up for
+     //ideal Vuforia Navigation
+     public Servo webCamServo;
+     public static final double CAM_SERVO_DOWN = 0.15;
+
+     //TensorFlow Object Detection
+     public TFObjectDetector tfObjectDetector;
+     private static final float MINIMUM_CONFIDENCE = 0.8f;
+
+     //Launcher
+     public DcMotor launchMotor;
+     public static final double LAUNCH_POWER = 0.715;
+
+     private static final long LAUNCH_ACCEL_TIME = 500;
+     //These variables were originally going to be used in both autonomous and teleOp
+     public double launchPositionX;
+     public double launchPositionY;
+     public float launchRotation;
+     public int reduceLaunchPos;
+
+     public boolean initLauncher;
+
+     //Ring Intake
+     public DcMotor collectionMotor;
+
+     //Ring Belt
+     public DcMotor ringBeltMotor;
+     public RevTouchSensor limitSwitch;
+     public int ringBeltStage;
+     public int ringBeltGap = 700;
+     public static final double RING_BELT_SLOW_POWER = 0.2;
+     public static final double RING_BELT_NORMAL_POWER = 0.6;
+     private int ringBeltPrev;
+     public long beltMaxStopTime;
+     public int beltReverseTicks;
+     public int beltMaxStopTicks;
+
+     //Wobble Goal Arm & Grabber
+     public DcMotor  wobbleArmMotor;
+     public Servo  wobbleGrabServo;
+     public int wobbleDownPos;
+     public int wobbleUpPos;
+     public int wobbleDropPos;
+     public static final double WOBBLE_SERVO_OPEN = 0;
+     public static final double WOBBLE_SERVO_CLOSED = 1;
+     public RevColorSensorV3 wobbleColorSensor;
+     public double wobbleScoreX;
+     public double wobbleScoreY;
+     public RevTouchSensor wobbleTouchSensor;
+
+     //Debugging
+     public double totalV;
+     private String TestingRecord = "Raw IMU, Delta, Saved";
+
+     public double traveledForward = 0;
+     public DcMotorEx motorAmpsTest;
+
+     //All our hardware initialization in one place, for everything that is the same in TeleOp and
+     //Autonomous
+     public void initHardware() {
+
+         //drive motors
+         driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
+         driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
+         driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
+         driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
+
+         driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+         driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+         driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+         driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+         //Localization encoders
+         encoderRight = hardwareMap.dcMotor.get("driveFrontRight");
+         encoderBack = hardwareMap.dcMotor.get("driveFrontLeft");
+         encoderLeft = hardwareMap.dcMotor.get("collect");
+
+         encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+         encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+         encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+         //init wobble arm
+         wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm");
+         wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+         wobbleArmMotor.setTargetPosition(0);
+         wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+         wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
+
+         wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
+         wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
+
+
+         //init collection motor
+         collectionMotor = hardwareMap.dcMotor.get("collect");
+         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);
+
+         limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
+
+         //init IMU
+         imu  = hardwareMap.get(BNO055IMU.class, "imu");
+
+         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+
+         parameters.mode = BNO055IMU.SensorMode.IMU;
+         parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+         parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+         parameters.loggingEnabled = false;
+
+         imu.initialize(parameters);
+
+         Velocity startVelocity = new Velocity();
+         startVelocity.xVeloc = 0;
+         startVelocity.yVeloc = 0;
+         startVelocity.zVeloc = 0;
+
+         Position startPosition = new Position();
+         startPosition.x = 0;
+         startPosition.y = 0;
+         startPosition.z = 0;
+
+         imu.startAccelerationIntegration(startPosition,startVelocity, 10);
+
+         //Init Launch Motor
+         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);
+         launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+         ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds");
+     }
+
+     //Localization Function! This function is represented in a flow diagram, earlier in the
+     //software section
+     //run this in every exec to track the robot's location.
+     public void updateLocation(){
+
+         // orientation is inverted to have clockwise be positive.
+         float imuAngle = -imu.getAngularOrientation().firstAngle;
+         double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
+
+         int encoderLeftCurrent = -encoderLeft.getCurrentPosition();
+         int encoderRightCurrent = encoderRight.getCurrentPosition();
+         int encoderBackCurrent = encoderBack.getCurrentPosition();
+
+         double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
+         double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
+         double encoderBackChange = encoderBackCurrent - encoderBackPrevious;
+
+         encoderLeftPrevious = encoderLeftCurrent;
+         encoderRightPrevious = encoderRightCurrent;
+         encoderBackPrevious = encoderBackCurrent;
+         rotationPrevious = imuAngle;
+
+         //The forward Vector has the luxury of having an odometer on both sides of the robot.
+         //This allows us to reduce the unwanted influence of turning the robot by averaging
+         //the two. unfortunatly we the current positioning of the odometers
+
+         //Since there isn't a second wheel to remove the influence of robot rotation, we have to
+         //instead do this by approximating the number of ticks that were removed due to rotation
+         //based on a separate calibration program.
+
+         double ticksPerDegreeForward;
+         double ticksPerDegreeSideways;
+
+         if (rotationChange < 0) {
+             ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
+             ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD;
+         } else {
+             ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
+             ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD;
+         }
+
+         forwardVector = ((encoderLeftChange+encoderRightChange)/2) -
+                 (rotationChange* ticksPerDegreeForward);
+
+         sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
+
+         double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
+         double direction = Math.toRadians(SampleRobotEx.rotation + (rotationChange/2)) +
+                 Math.atan2(sidewaysVector,forwardVector);
+
+         double xChange = magnitude * (Math.sin(direction));
+         double yChange = magnitude * (Math.cos(direction));
+
+         locationX += xChange;
+         locationY += yChange;
+
+         SampleRobotEx.rotation += rotationChange;
+         SampleRobotEx.rotation = scaleAngleRange(SampleRobotEx.rotation);
+
+     }
+
+     public float getRotation() {
+         return SampleRobotEx.rotation;
+     }
+
+     public double getLocationX() {
+         return SampleRobotEx.locationX;
+     }
+
+     public double getLocationY() {
+         return SampleRobotEx.locationY;
+     }
+
+     //This is meant to only be used to indicate starting positions and to reorient the robot.
+     public void setLocalization(float rotation, double x, double y) {
+         SampleRobotEx.rotation = rotation;
+         SampleRobotEx.locationX = x;
+         SampleRobotEx.locationY = y;
+     }
+
+     //returns the angle from the robot's current position to the given target position.
+     public float getAngleToPosition (double x, double y) {
+         double differenceX = x- getLocationX();
+         double differenceY = y- getLocationY();
+         double angle = Math.toDegrees(Math.atan2(differenceX, differenceY ));
+
+         return (float) angle;
+     }
+
+     //Unit conversions
+     public double ticksToInches(double ticks) {
+         return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
+     }
+
+     public double inchesToTicks(double inches) {
+         return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE);
+
+     }
+
+     //Returns the shortest angle between two directions, with positive angles indicating that the
+     // reference is to the right (clockwise) of the current. Negative angles indicate that the
+     // reference is to the left.
+     public float getRelativeAngle(float reference, float current) {
+         return scaleAngleRange(current - reference);
+     }
+
+     //Drive Functions
+     public void setDrivePower(double powerFrontLeft, double powerFrontRight,
+                               double powerBackLeft, double powerBackRight){
+         driveFrontLeft.setPower(powerFrontLeft);
+         driveFrontRight.setPower(powerFrontRight);
+         driveBackLeft.setPower(powerBackLeft);
+         driveBackRight.setPower(powerBackRight);
+     }
+
+     public void setDrivePower(double[] powers){
+         driveFrontLeft.setPower(powers[0]);
+         driveFrontRight.setPower(powers[1]);
+         driveBackLeft.setPower(powers[2]);
+         driveBackRight.setPower(powers[3]);
+     }
+
+     //returns an array of the powers necessary to execute the provided motion.
+     //"degreesDirectionMotion" is the angle relative to the field that the robot should drive at.
+     //"degreesDirectionFace" is the angle the robot should face relative to the field. The order of
+     //the output powers is ForwardLeft, ForwardRight, BackLeft, BackRight
+     public double[] getMecanumPowers(float degreesDirectionMotion, double scalar,
+                                      float degreesDirectionFace) {
+         angularVelocity = imu.getAngularVelocity().xRotationRate;
+
+         //calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion
+         //once it is pointed towards the degreesDirectionFace
+         double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion));
+         double y = Math.cos(rad);
+         double x = Math.sin(rad);
+
+         double p = y + x;
+         double q = y - x;
+
+
+
+         //calculating correction needed to steer the robot towards the degreesDirectionFace
+         float relativeRotation =
+                 getRelativeAngle(degreesDirectionFace, SampleRobotEx.rotation);
+         double turnCorrection =
+                 Math.pow(CUBIC_CORRECTION * relativeRotation, 3) +
+                         LINEAR_CORRECTION * relativeRotation;
+
+         if (relativeRotation != 0) {
+             double momentumRelative =  angularVelocity *
+                     (relativeRotation / Math.abs(relativeRotation));
+             double exponential =
+                     Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+             double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
+             //reduces concern for  momentum when the angle is far away from target
+             turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) *
+                     (1  - momentumCorrection)) / 180 );
+         }
+
+         double powerForwardRight = scalar * (q + turnCorrection);
+         double powerForwardLeft = scalar * (p - turnCorrection);
+         double powerBackRight = scalar * (p + turnCorrection);
+         double powerBackLeft = scalar * (q - turnCorrection);
+
+
+         // The "extreme" is the power value that is furthest from zero. When this values exceed the
+         // -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the
+         // workable range without altering the final motion vector.
+
+         double extreme = Math.max(
+                 Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)),
+                 Math.max(Math.abs(powerBackRight),Math.abs(powerBackLeft)));
+
+         if (extreme > 1) {
+             powerForwardRight = powerForwardRight/extreme;
+             powerForwardLeft = powerForwardLeft/extreme;
+             powerBackRight = powerBackRight/extreme;
+             powerBackLeft = powerBackLeft/extreme;
+         }
+
+         double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
+
+         double totalPower = Math.abs(powerForwardLeft) +
+                 Math.abs(powerForwardRight) +
+                 Math.abs(powerBackLeft) +
+                 Math.abs(powerBackRight);
+         if (totalPower < ZERO_POWER_THRESHOLD) {
+             powers = new double[] {0,0,0,0};
+         }
+
+
+         return powers;
+     }
+     //Outputs the power necessary to turn and face a provided direction
+     public double[] getFacePowers(float direction, double power) {
+         angularVelocity = imu.getAngularVelocity().xRotationRate;
+         double relativeAngle = getRelativeAngle(direction, SampleRobotEx.rotation);
+         double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) +
+                 FACE_LINEAR_CORRECTION * relativeAngle;
+
+         if (relativeAngle > 0.5) {
+             scaler += FACE_MIN_CORRECTION;
+         } else if (relativeAngle < -0.5) {
+             scaler -= FACE_MIN_CORRECTION;
+         }
+
+         if (relativeAngle != 0) {
+             double momentumRelative =  angularVelocity * (relativeAngle / Math.abs(relativeAngle));
+             double exponential = Math.pow(FACE_MOMENTUM_CORRECTION,
+                     FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+             double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
+
+             scaler *= momentumCorrection;
+         }
+
+         double left = -power * scaler;
+         double right = power *scaler;
+
+         double[] powers = {left,right};
+
+         double totalPower = 2 * (Math.abs(left) + Math.abs(right));
+         if (totalPower < ZERO_POWER_THRESHOLD) {
+             powers = new double[] {0,0};
+         }
+
+         return powers;
+     }
+
+     public float scaleAngleRange(float angle) {
+         if (angle < -180) {
+             angle += 360;
+         }
+         if (angle > 180) {
+             angle -= 360;
+         }
+         return angle;
+     }
+
+ }

TeamCode/src/main/java/org/timecrafters/javaClass/samples/SpencerFirstEngine.java

@@ -0,0 +1,27 @@
+package org.timecrafters.javaClass.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.javaClass.aubrey.AubreyFirstState;
+import org.timecrafters.javaClass.spencer.SpencerFirstState;
+
+@Autonomous (name = "Spencer: First Program", group = "spencer")
+public class SpencerFirstEngine extends CyberarmEngine {
+    
+    SampleRobot robot;
+
+    @Override
+    public void init() {
+        robot = new SampleRobot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
+        super.init();
+    }
+    
+    @Override
+    public void setup() {
+        addState(new SpencerFirstState(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/javaClass/spencer/SpencerFirstState.java

@@ -0,0 +1,22 @@
+package org.timecrafters.javaClass.spencer;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.javaClass.samples.SampleRobot;
+
+public class SpencerFirstState extends CyberarmState {
+
+    //here, you'll find some of your variables. you can add more as you need them.
+    private SampleRobot robot;
+
+    //This is the constructor. It lets other code bits run use the code you put here
+    public SpencerFirstState(SampleRobot robot) {
+        this.robot = robot;
+    }
+
+    //This is a method. methods are bits of code that can be run elsewhere.
+    //This one is set up to repeat every few milliseconds
+    @Override
+    public void exec() {
+
+    }
+}

build.gradle

@@ -15,7 +15,7 @@ buildscript {
         jcenter()
     }
     dependencies {
-        classpath 'com.android.tools.build:gradle:4.0.1'
+        classpath 'com.android.tools.build:gradle:4.1.2'
     }
 }
 

gradle/wrapper/gradle-wrapper.properties

@@ -1,6 +1,6 @@
-#Fri Jul 24 14:30:03 PDT 2020
+#Sat Jul 24 09:32:10 CDT 2021
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.1.1-all.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.5-bin.zip