added second direction to the third odometer. Set up DriveToCoordinates, ThreadStates, and TeleOp. Cleaned up some obsolete states

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

@@ -0,0 +1,72 @@
+package org.timecrafters.UltimateGoal.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+public class DriveToCoordinates extends CyberarmState {
+
+    private Robot robot;
+    private String groupName = "manual";
+    private String actionName;
+    private double xTarget;
+    private double yTarget;
+    private float faceAngle;
+    private double tolerance;
+    private double power;
+    private boolean braking;
+    private long breakStartTime;
+    private long breakTime;
+
+    public DriveToCoordinates(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    public DriveToCoordinates(double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) {
+        this.xTarget = xTarget;
+        this.yTarget = yTarget;
+        this.faceAngle = faceAngle;
+        this.tolerance = tolerance;
+        this.power = power;
+        this.breakTime = breakTime;
+    }
+
+    @Override
+    public void init() {
+        if (!groupName.equals("manual")) {
+            xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "x").value());
+            yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "y").value());
+            power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
+            faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
+            tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tol").value());
+            breakTime = robot.stateConfiguration.variable(groupName, actionName, "breakMS").value();
+        }
+    }
+
+    @Override
+    public void start() {
+
+    }
+
+    @Override
+    public void exec() {
+
+        double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
+
+        if (distanceToTarget > tolerance) {
+            double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
+            robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
+            braking = false;
+        } else {
+            if (!braking) {
+                breakStartTime = System.currentTimeMillis();
+                robot.setBrakePosAll();
+                braking = true;
+            }
+            robot.brakeAll();
+            setHasFinished(System.currentTimeMillis() - breakStartTime > breakTime);
+        }
+
+    }
+}

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

@@ -0,0 +1,33 @@
+package org.timecrafters.UltimateGoal.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+import java.util.ArrayList;
+
+public class ThreadStates extends CyberarmState {
+
+    private ArrayList<CyberarmState> states = new ArrayList<>();
+
+    public ThreadStates(ArrayList<CyberarmState> states) {
+        this.states = states;
+    }
+
+    @Override
+    public void start() {
+        for (CyberarmState state : states) {
+            addParallelState(state);
+        }
+    }
+
+    @Override
+    public void exec() {
+        int finishedStates = 0;
+        for (CyberarmState state : states) {
+            if (state.getHasFinished()) {
+                finishedStates += 1;
+            }
+        }
+        setHasFinished(finishedStates == states.size());
+
+    }
+}

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

@@ -3,14 +3,13 @@ package org.timecrafters.UltimateGoal.Calibration;
 import org.cyberarm.engine.V2.CyberarmState;
 import org.timecrafters.UltimateGoal.Robot;
 
-import java.util.ArrayList;
-
 public class OdometryCalibration extends CyberarmState {
 
     private Robot robot;
     private float rotation;
     private int currentTick;
-    private double ticksPerDegree;
+    private double ticksPerDegreeClockwise;
+    private double ticksPerDegreeCounterClockwise;
 
     public OdometryCalibration(Robot robot) {
         this.robot = robot;
@@ -19,21 +18,27 @@ public class OdometryCalibration extends CyberarmState {
     @Override
     public void exec() {
 
+        double power = 0.1;
+        rotation = -robot.imu.getAngularOrientation().firstAngle;
+        currentTick = robot.encoderBack.getCurrentPosition();
+
         if (engine.gamepad1.x) {
-            double power = 0.1;
             robot.setDrivePower(power, -power, power, -power);
-            currentTick = robot.encoderBack.getCurrentPosition();
+            ticksPerDegreeClockwise = currentTick/rotation;
-            rotation = -robot.imu.getAngularOrientation().firstAngle;
+        } else if(engine.gamepad1.y) {
-            ticksPerDegree = currentTick/rotation;
+            robot.setDrivePower(-power, power, -power, power);
+            ticksPerDegreeCounterClockwise = currentTick/rotation;
         } else {
             robot.setDrivePower(0,0,0,0);
         }
+
     }
 
     @Override
     public void telemetry() {
         engine.telemetry.addData("degrees", rotation);
         engine.telemetry.addData("ticks", currentTick);
-        engine.telemetry.addData("ticks per degree", ticksPerDegree);
+        engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise);
+        engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise);
     }
 }

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

@@ -23,7 +23,7 @@ public class StalPowerCalibrator extends CyberarmState {
 
     @Override
     public void init() {
-        Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
+//        Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
         Powers = new double[] {0,0,0,0};
 
         for (DcMotor motor : Motors) {

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

@@ -1,27 +1,78 @@
 package org.timecrafters.UltimateGoal.HardwareTesting;
 
+import com.qualcomm.hardware.bosch.BNO055IMU;
+
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
 import org.timecrafters.UltimateGoal.Robot;
 
 
 public class ControlHubTest extends CyberarmState {
 
     private Robot robot;
-    private float angle = 0;
+
+    private Acceleration acceleration;
+    private double Vx = 0;
+    private double Vy = 0;
+    private double Vz = 0;
+    private double aVx = 0;
+    private double aVy = 0;
+    private double aVz = 0;
+
 
     public ControlHubTest(Robot robot) {
         this.robot = robot;
     }
 
+    @Override
+    public void init() {
+
+        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;
+
+        robot.imu.startAccelerationIntegration(startPosition,startVelocity, 200);
+    }
+
     @Override
     public void exec() {
-        angle = robot.imu.getAngularOrientation().firstAngle;
+        acceleration = robot.imu.getAcceleration();
+
+        Vx = robot.imu.getVelocity().xVeloc;
+        Vy = robot.imu.getVelocity().yVeloc;
+        Vz = robot.imu.getVelocity().zVeloc;
+
+        aVx = robot.imu.getAngularVelocity().xRotationRate;
+        aVy = robot.imu.getAngularVelocity().yRotationRate;
+        aVz = robot.imu.getAngularVelocity().zRotationRate;
+
+
     }
 
     @Override
     public void telemetry() {
-        engine.telemetry.addLine("Greetings");
+        engine.telemetry.addLine("Acceleration");
-        engine.telemetry.addData("Angle", angle);
+        engine.telemetry.addData("x", acceleration.xAccel);
+        engine.telemetry.addData("y", acceleration.yAccel);
+        engine.telemetry.addData("z", acceleration.zAccel);
+
+        engine.telemetry.addLine("Velocity");
+        engine.telemetry.addData("X", Vx);
+        engine.telemetry.addData("Y", Vy);
+        engine.telemetry.addData("Z", Vz);
+
+        engine.telemetry.addLine("Angular Velocity");
+        engine.telemetry.addData("X", aVx);
+        engine.telemetry.addData("Y", aVy);
+        engine.telemetry.addData("Z", aVz);
 
     }
 }

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

@@ -1,66 +0,0 @@
-package org.timecrafters.UltimateGoal.HardwareTesting;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
-
-
-public class ControlerTest extends CyberarmState {
-
-    private float leftJoystickDegrees;
-    private double leftJoystickMagnitude;
-    private float rightJoystickDegrees;
-    private double rightJoystickMagnitude;
-    private double leftJoystickX;
-    private double leftJoystickY;
-    private double rightJoystickX;
-    private double rightJoystickY;
-
-
-
-
-    @Override
-    public void exec() {
-
-        leftJoystickX = engine.gamepad1.left_stick_x;
-        leftJoystickY = engine.gamepad1.left_stick_y;
-
-        leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
-        leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
-
-        rightJoystickX = engine.gamepad1.left_stick_x;
-        rightJoystickY = engine.gamepad1.left_stick_y;
-
-        rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
-        rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
-
-
-
-    }
-
-    @Override
-    public void telemetry() {
-
-        engine.telemetry.addLine("Left Joystick");
-        engine.telemetry.addData("Pos", "("+leftJoystickX+","+leftJoystickY+")");
-        engine.telemetry.addData("Angle", leftJoystickDegrees);
-        engine.telemetry.addData("Mag", leftJoystickMagnitude);
-
-        engine.telemetry.addLine("Right Joystick");
-        engine.telemetry.addData("Pos", "("+rightJoystickX+","+rightJoystickY+")");
-        engine.telemetry.addData("Angle", leftJoystickDegrees);
-        engine.telemetry.addData("Mag", leftJoystickMagnitude);
-
-        engine.telemetry.addLine("Buttons");
-        engine.telemetry.addData("a", engine.gamepad1.a);
-        engine.telemetry.addData("b", engine.gamepad1.b);
-        engine.telemetry.addData("x", engine.gamepad1.x);
-        engine.telemetry.addData("y", engine.gamepad1.y);
-
-        engine.telemetry.addLine("Top");
-        engine.telemetry.addData("Left Bump", engine.gamepad1.left_bumper);
-        engine.telemetry.addData("Right Bump", engine.gamepad1.right_bumper);
-        engine.telemetry.addData("Left Trigger", engine.gamepad1.left_trigger);
-        engine.telemetry.addData("Right Trigger", engine.gamepad1.right_trigger);
-
-    }
-}

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

@@ -16,7 +16,5 @@ public class EncoderTest extends CyberarmState {
     @Override
     public void exec() {
 
-        robot.setDrivePower(0.1, 0.1,0.1,0.1);
-
     }
 }

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

@@ -1,31 +0,0 @@
-package org.timecrafters.UltimateGoal.HardwareTesting;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
-
-
-public class LauncherTest extends CyberarmState {
-
-    DcMotor LaunchMotor;
-    DcMotor SuckMotor;
-
-    @Override
-    public void init() {
-        LaunchMotor = engine.hardwareMap.dcMotor.get("launcher");
-        SuckMotor = engine.hardwareMap.dcMotor.get("collector");
-    }
-
-    @Override
-    public void exec() {
-        LaunchMotor.setPower(1);
-        SuckMotor.setPower(1);
-    }
-
-    @Override
-    public void telemetry() {
-
-
-    }
-}

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

@@ -1,6 +1,9 @@
 package org.timecrafters.UltimateGoal.HardwareTesting;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
 import org.timecrafters.UltimateGoal.Robot;
 
 
@@ -16,15 +19,43 @@ public class MecanumFunctionTest extends CyberarmState {
     private double powerBackLeft=0;
     private double powerBackRight=0;
 
-    private static double TURN_POWER_SCALE = 0.5;
+    private double aVx = 0;
+    private double aVy = 0;
+    private double aVz = 0;
+
+    private int powerStep = 5;
+    private double POWER_SCALE = 0.5 ;
+    private boolean toggleSpeedInput = false;
 
     public MecanumFunctionTest(Robot robot) {
         this.robot = robot;
     }
 
+    @Override
+    public void init() {
+        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;
+
+        robot.imu.startAccelerationIntegration(startPosition,startVelocity, 10);
+    }
+
     @Override
     public void exec() {
         robot.updateLocation();
+        robot.record();
+
+        AngularVelocity angularVelocity = robot.imu.getAngularVelocity();
+
+        aVx = angularVelocity.xRotationRate;
+        aVy = angularVelocity.yRotationRate;
+        aVz = angularVelocity.zRotationRate;
 
         double leftJoystickX = engine.gamepad1.left_stick_x;
         double leftJoystickY = engine.gamepad1.left_stick_y;
@@ -37,29 +68,45 @@ public class MecanumFunctionTest extends CyberarmState {
 
         rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
         rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
-//
+
         powerFrontLeft = 0;
         powerFrontRight = 0;
         powerBackLeft = 0;
         powerBackRight = 0;
 
+        boolean a = engine.gamepad1.a;
+        boolean b = engine.gamepad1.b;
+
+        if (a && !toggleSpeedInput && POWER_SCALE < 1) {
+            powerStep += 1;
+            POWER_SCALE = powerStep * 0.1;
+        }
+
+        if (b && !toggleSpeedInput && POWER_SCALE > 0.1) {
+            powerStep -= 1;
+            POWER_SCALE = powerStep * 0.1;
+        }
+
+        toggleSpeedInput = a || b;
+
         if (rightJoystickMagnitude == 0) {
             if (leftJoystickMagnitude !=0) {
-                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees);
+                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, leftJoystickDegrees);
                 powerFrontLeft = powers[0];
                 powerFrontRight = powers[1];
                 powerBackLeft = powers[2];
                 powerBackRight = powers[3];
+
             }
         } else {
             if (leftJoystickMagnitude == 0) {
-                double[] powers =  robot.getFacePowers(rightJoystickDegrees, rightJoystickMagnitude);
+                double[] powers =  robot.getFacePowers(rightJoystickDegrees, POWER_SCALE * rightJoystickMagnitude);
-                powerFrontLeft = TURN_POWER_SCALE * powers[0];
+                powerFrontLeft = powers[0];
-                powerFrontRight = TURN_POWER_SCALE * powers[1];
+                powerFrontRight = powers[1];
-                powerBackLeft = TURN_POWER_SCALE * powers[0];
+                powerBackLeft = powers[0];
-                powerBackRight = TURN_POWER_SCALE * powers[1];
+                powerBackRight = powers[1];
             } else {
-                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, rightJoystickDegrees);
+                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, rightJoystickDegrees);
                 powerFrontLeft = powers[0];
                 powerFrontRight = powers[1];
                 powerBackLeft = powers[2];
@@ -67,22 +114,24 @@ public class MecanumFunctionTest extends CyberarmState {
             }
         }
 
-//        robot.record(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
-
         robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
 
+
     }
 
     @Override
     public void telemetry() {
+        engine.telemetry.addData("Scale", POWER_SCALE);
 
-        engine.telemetry.addLine("Left Joystick");
+        engine.telemetry.addLine("Angular Velocity");
-        engine.telemetry.addData("Angle", leftJoystickDegrees);
+        engine.telemetry.addData("X", aVx);
-        engine.telemetry.addData("Mag", leftJoystickMagnitude);
+        engine.telemetry.addData("Y", aVy);
-
+        engine.telemetry.addData("Z", aVz);
-        engine.telemetry.addLine("Right Joystick");
-        engine.telemetry.addData("Angle", rightJoystickDegrees);
-        engine.telemetry.addData("Mag", rightJoystickMagnitude);
 
+        engine.telemetry.addLine("Powers");
+        engine.telemetry.addData("FL", robot.driveFrontLeft.motor.getPower());
+        engine.telemetry.addData("FR", robot.driveFrontRight.motor.getPower());
+        engine.telemetry.addData("BL", robot.driveBackLeft.motor.getPower());
+        engine.telemetry.addData("BR", robot.driveBackRight.motor.getPower());
     }
 }

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

@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.UltimateGoal.Robot;
 
-@TeleOp (name = "Encoder test", group = "test")
+@TeleOp (name = "Performance test", group = "test")
 public class TestingEngine extends CyberarmEngine {
 
     private Robot robot;
@@ -24,9 +24,9 @@ public class TestingEngine extends CyberarmEngine {
         addState(new MecanumFunctionTest(robot));
     }
 
-//    @Override
+    @Override
-//    public void stop() {
+    public void stop() {
-//        robot.saveRecording();
+        robot.saveRecording();
-//        super.stop();
+        super.stop();
-//    }
+    }
 }

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

@@ -1,30 +0,0 @@
-package org.timecrafters.UltimateGoal.HardwareTesting;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.cyberarm.engine.V2.CyberarmState;
-
-
-public class WelcomeToJankyTown extends CyberarmState {
-
-    DcMotor left;
-    DcMotor right;
-
-    @Override
-    public void init() {
-        left = engine.hardwareMap.dcMotor.get("left");
-        right = engine.hardwareMap.dcMotor.get("right");
-    }
-
-    @Override
-    public void exec() {
-        left.setPower(-engine.gamepad1.left_stick_y);
-        right.setPower(engine.gamepad1.right_stick_y);
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addLine("Welcome to Janky Town!");
-
-    }
-}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java

@@ -38,7 +38,7 @@ public class IMUDrive extends CyberarmState {
         }
 
         angleTarget=robot.getRotation();
-        tickStart = robot.driveFrontRight.getCurrentPosition();
+        tickStart = robot.driveFrontRight.motor.getCurrentPosition();
     }
 
     @Override
@@ -46,7 +46,7 @@ public class IMUDrive extends CyberarmState {
 
         robot.updateLocation();
 
-        int ticksTraveled = Math.abs( robot.driveFrontRight.getCurrentPosition()-tickStart);
+        int ticksTraveled = Math.abs( robot.driveFrontRight.motor.getCurrentPosition()-tickStart);
         if (ticksTraveled > tickTarget) {
 //            robot.setDrivePower(0,0);
             sleep(finishDelay);

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

@@ -14,6 +14,8 @@ 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;
@@ -57,25 +59,22 @@ public class   Robot {
     public DcMotor encoderRight;
     public DcMotor encoderBack;
 
-    private int brakePosFrontLeft;
+    public DcMotor launcher;
-    private int brakePosFrontRight;
-    private int brakePosBackLeft;
-    private int brakePosBackRight;
 
-    static final double BIAS_FRONT_LEFT = 1;
+    //Steering Constants
-    static final double BIAS_FRONT_RIGHT = 1;
+    static final double FINE_CORRECTION = 0.05 ;
-    static final double BIAS_BACK_LEFT = 1;
+    static final double LARGE_CORRECTION = 0.002;
-    static final double BIAS_BACK_RIGHT = 1;
+    static final double MOMENTUM_CORRECTION = 1.015;
-
+    static final double MOMENTUM_MAX_CORRECTION = 1.35;
-    static final double FINE_CORRECTION = 0.01;
+    static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
-    static final double LARGE_CORRECTION = 0.01;
 
     //Conversion Constants
     static final double ENCODER_CIRCUMFERENCE = Math.PI * 4;
     static final int COUNTS_PER_REVOLUTION = 8192;
     static final float mmPerInch = 25.4f;
     //todo make/run calibration code to find this exact value
-    static final double TICKS_PER_ROBOT_DEGREE = Math.PI * 1000;
+    static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = Math.PI * 1000;
+    static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = Math.PI * 1000;
 
     // Inches Forward of axis of rotation
     static final float CAMERA_FORWARD_DISPLACEMENT  = 13f;
@@ -89,28 +88,48 @@ public class   Robot {
     public double locationY;
     private float rotation;
 
+    private int encoderLeftPrevious = 0;
+    private int encoderBackPrevious = 0;
+    private int encoderRightPrevious = 0;
+    private float rotationPrevious = 0;
+    public float angularVelocity;
+
+    //Launcher
+    public DcMotor launchMotor;
+
+    public static final double LAUNCH_POSITION_X = 28 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
+    public static final double LAUNCH_POSITION_Y = 0 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
+    public static final float LAUNCH_ROTATION = 0;
+    public static final double LAUNCH_TOLERANCE_POS = 50;
+    public static final double LAUNCH_TOLERANCE_FACE = 0.5;
+
+    //Ring Belt
+    public static final int RING_BELT_LOOP_TICKS = 1000;
+    public static final int RING_BELT_POS_1 = 200;
+    public static final int RING_BELT_POS_2 = 400;
+    public static final int RING_BELT_POS_3 = 600;
+
     //Debugging
     public double visionX;
     public double visionY;
     public float rawAngle;
 //    private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
-    private String TestingRecord = "Rotation";
+    private String TestingRecord = "AngularVelocity";
 
     public double traveledLeft;
     public double traveledRight;
 
 
-    private int encoderLeftPrevious = 0;
-    private int encoderBackPrevious = 0;
-    private int encoderRightPrevious = 0;
-    private float rotationPrevious = 0;
-
     //vuforia navigation
     public boolean trackableVisible;
     private VuforiaTrackables targetsUltimateGoal;
     private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
     private OpenGLMatrix lastConfirmendLocation;
 
+    private long timeStartZeroVelocity = 0;
+    private static final long MIN_CHECK_DURATION_MS = 500;
+    private static final double MIN_CHECK_VELOCITY = 0.005;
+
     //TensorFlow Object Detection
     public TFObjectDetector tfObjectDetector;
     private static final float MINIMUM_CONFIDENCE = 0.8f;
@@ -126,16 +145,24 @@ public class   Robot {
         MotorConfigurationType motorType = dcMotor.getMotorType();
         motorType.setGearing(5);
         motorType.setTicksPerRev(140);
-        motorType.setMaxRPM(1200);
+        motorType.setMaxRPM(800);
 
         driveFrontLeft = new DriveMotor(dcMotor, motorType, DcMotorSimple.Direction.FORWARD);
         driveFrontRight = new DriveMotor(hardwareMap.dcMotor.get("driveFrontRight"), motorType, DcMotorSimple.Direction.REVERSE);
         driveBackLeft = new DriveMotor(hardwareMap.dcMotor.get("driveBackLeft"), motorType, DcMotorSimple.Direction.FORWARD);
         driveBackRight = new DriveMotor(hardwareMap.dcMotor.get("driveBackRight"), motorType, DcMotorSimple.Direction.REVERSE);
 
+        driveFrontLeft.init();
+        driveFrontRight.init();
+        driveBackLeft.init();
+        driveBackRight.init();
+
+        //todo add these when they exist
         encoderLeft = hardwareMap.dcMotor.get("odoLeft");
-        encoderRight = hardwareMap.dcMotor.get("odoRight");
+//        encoderRight = hardwareMap.dcMotor.get("odoRight");
-        encoderBack = hardwareMap.dcMotor.get("odoBack");
+//        encoderBack = hardwareMap.dcMotor.get("odoBack");
+//
+//        launcher = hardwareMap.dcMotor.get("launcher");
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
@@ -146,6 +173,18 @@ public class   Robot {
 
         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);
+
 //        initVuforia();
 
         rotation = stateConfiguration.variable("system", "startPos", "direction").value();
@@ -216,10 +255,12 @@ public class   Robot {
 
     //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);
 
+        //todo add this when encoders exist.
         int encoderLeftCurrent = encoderLeft.getCurrentPosition();
         int encoderRightCurrent = encoderRight.getCurrentPosition();
         int encoderBackCurrent = encoderBack.getCurrentPosition();
@@ -241,7 +282,16 @@ public class   Robot {
         //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 sidewaysVector = encoderBackChange + (rotationChange*TICKS_PER_ROBOT_DEGREE);
+
+        double ticksPerDegree;
+
+        if(rotationChange < 0) {
+            ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
+        } else {
+            ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
+        }
+        //todo:  check if direction of the back encoder gives expected values
+        double sidewaysVector = encoderBackChange - (rotationChange* ticksPerDegree);
 
         double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
         double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
@@ -251,9 +301,26 @@ public class   Robot {
 
         locationX += xChange;
         locationY += yChange;
+
         rotation += rotationChange;
 
 
+        double totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightCurrent) + Math.abs(encoderBackChange);
+
+        if (totalV < MIN_CHECK_VELOCITY) {
+            long timeCurrent = System.currentTimeMillis();
+
+            if (timeStartZeroVelocity == 0) {
+                timeStartZeroVelocity = timeCurrent;
+            } else if (timeCurrent - timeStartZeroVelocity >= MIN_CHECK_DURATION_MS) {
+                syncWithVuforia();
+            }
+
+        } else {
+            timeStartZeroVelocity = 0;
+        }
+
+
         if (rotation > 180) {
             rotation -= 360;
         }
@@ -263,6 +330,15 @@ public class   Robot {
 
     }
 
+    public void checkVuforiaSync() {
+
+        double V1 = driveFrontLeft.motor.getPower();
+        double V2 = driveFrontRight.motor.getPower();
+        double V3 = driveBackLeft.motor.getPower();
+        double V4 = driveBackRight.motor.getPower();
+
+    }
+
     public void syncWithVuforia() {
         trackableVisible = false;
         for (VuforiaTrackable trackable : trackables) {
@@ -354,10 +430,10 @@ public class   Robot {
 
     //Drive Functions
     public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){
-        driveFrontLeft.setPower(powerFrontLeft * BIAS_FRONT_LEFT);
+        driveFrontLeft.setPower(powerFrontLeft);
-        driveFrontRight.setPower(powerFrontRight * BIAS_FRONT_RIGHT);
+        driveFrontRight.setPower(powerFrontRight);
-        driveBackLeft.setPower(powerBackLeft * BIAS_BACK_LEFT);
+        driveBackLeft.setPower(powerBackLeft);
-        driveBackRight.setPower(powerBackRight * BIAS_BACK_RIGHT);
+        driveBackRight.setPower(powerBackRight);
     }
 
     //returns an array of the powers necessary to execute the provided motion. "degreesDirectionMotion"
@@ -365,25 +441,52 @@ public class   Robot {
     //the angle the robot should face relative to the field. The order of the output powers is
     //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 = scalar * Math.cos(rad);
+        double y = Math.cos(rad);
-        double x = scalar * Math.sin(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, rotation);
         double turnCorrection = Math.pow(LARGE_CORRECTION * relativeRotation, 3) + FINE_CORRECTION * relativeRotation;
 
-        double powerForwardRight = q + turnCorrection;
+        double powerForwardRight = scalar * (q + turnCorrection);
-        double powerForwardLeft = p - turnCorrection;
+        double powerForwardLeft = scalar * (p - turnCorrection);
-        double powerBackRight = p + turnCorrection;
+        double powerBackRight = scalar * (p + turnCorrection);
-        double powerBackLeft = q - turnCorrection;
+        double powerBackLeft = scalar * (q - turnCorrection);
 
+        //the turnCorrection often results in powers with magnitudes significantly larger than the
+        //scalar. The scaleRatio mitigates this without altering the quality of the motion by making
+        //it so that the average of the four magnitudes is equal to the scalar magnitude.
+        double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) +
+                Math.abs(powerBackRight) + Math.abs(powerBackLeft);
+        double scaleRatio = (4 * Math.abs(scalar))/powerSum;
+
+        powerForwardRight *= scaleRatio;
+        powerForwardLeft *= scaleRatio;
+        powerBackRight *= scaleRatio;
+        powerBackLeft *= scaleRatio;
+
+
+        if (relativeRotation != 0) {
+            double momentumRelative =  angularVelocity * (relativeRotation / Math.abs(relativeRotation));
+            double exponential = Math.pow(MOMENTUM_CORRECTION, -momentumRelative);
+            double momentumCorrection = (2*exponential)/(1+exponential);
+            powerForwardRight *= momentumCorrection;
+            powerForwardLeft *= momentumCorrection;
+            powerBackRight *= momentumCorrection;
+            powerBackLeft *= momentumCorrection;
+        }
 
         // The "extreme" is the power value that is furthest from zero. When this values exceed the
         // -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the
-        // workable range without altering the final motion vector;
+        // workable range without altering the final motion vector.
 
         double extreme = Math.max(
                 Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)),
@@ -396,6 +499,15 @@ public class   Robot {
             powerBackLeft = powerBackLeft/extreme;
         }
 
+//        double powerControlThreshold = 0.6 * ;
+//
+//        if (Math.min(extreme, 1) > powerControlThreshold) {
+//            powerForwardLeft *= powerControlThreshold;
+//            powerForwardRight *= powerControlThreshold;
+//            powerBackLeft *= powerControlThreshold;
+//            powerBackRight *= powerControlThreshold;
+//        }
+
         double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
 
         return powers;
@@ -409,7 +521,7 @@ public class   Robot {
     }
 
     //sets the position the motors will lock to when braking to the motor's current position.
-    public void setAllBrakePos() {
+    public void setBrakePosAll() {
         driveFrontLeft.setBrakePosition();
         driveFrontRight.setBrakePosition();
         driveBackLeft.setBrakePosition();
@@ -418,17 +530,21 @@ public class   Robot {
 
     //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, rotation);
         double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle;
 
+        if (relativeAngle != 0) {
+            double momentumRelative =  angularVelocity * (relativeAngle / Math.abs(relativeAngle));
+            double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+            double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
+
+            scaler *= momentumCorrection;
+        }
+
         double left = -power * scaler;
         double right = power *scaler;
 
-
-//        if (relativeAngle > 0) {
-//            left *= -1;
-//            right *= -1;
-//        }
         double[] powers = {left,right};
         return powers;
     }
@@ -442,7 +558,7 @@ public class   Robot {
         //towards the target angle
         //--------------------------------------------------------------------------------------
 
-        double turnPowerCorrection = Math.pow(0.03 * relativeAngle, 3) + 0.02 * relativeAngle;
+        double turnPowerCorrection = Math.abs(power) * (Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle);
 
         //Adjusts power based on degrees off from target.
         double leftPower = power - turnPowerCorrection;
@@ -463,7 +579,7 @@ public class   Robot {
 //    }
 
     public void record() {
-        TestingRecord+="\n"+rotation;
+        TestingRecord+="\n"+angularVelocity;
     }
 
     public void saveRecording() {

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

@@ -1,4 +1,10 @@
 package org.timecrafters.UltimateGoal;
 
-public class TeleOpEngine {
+import org.cyberarm.engine.V2.CyberarmEngine;
+
+public class TeleOpEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+
+    }
 }

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

@@ -1,4 +1,83 @@
 package org.timecrafters.UltimateGoal;
 
-public class TeleOpState {
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Autonomous.DriveToCoordinates;
+
+public class TeleOpState extends CyberarmState {
+
+    private Robot robot;
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+
+    private double POWER_SPRINT = 0.4;
+    private double POWER_NORMAL = 0.2;
+    private double powerScale = 0.2 ;
+    private boolean toggleSpeedInput = false;
+
+    private boolean launchInput = false;
+    private CyberarmState launchDriveState = new DriveToCoordinates(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y, robot.LAUNCH_ROTATION, 10, POWER_NORMAL, 50);
+
+    public TeleOpState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        double leftJoystickX = engine.gamepad1.left_stick_x;
+        double leftJoystickY = engine.gamepad1.left_stick_y;
+
+        leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
+        leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+        double rightJoystickX = engine.gamepad1.right_stick_x;
+        double rightJoystickY = engine.gamepad1.right_stick_y;
+
+        rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
+        rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+        double[] powers = {0,0,0,0};
+
+        boolean joystickButton = engine.gamepad1.left_stick_button;
+
+        if (joystickButton && !toggleSpeedInput) {
+            if (powerScale == POWER_NORMAL) {
+                powerScale = POWER_SPRINT;
+            } else {
+                powerScale = POWER_NORMAL;
+            }
+        }
+
+        toggleSpeedInput = joystickButton;
+
+
+        if (rightJoystickMagnitude == 0) {
+            if (leftJoystickMagnitude !=0) {
+                powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees);
+            }
+        } else {
+            if (leftJoystickMagnitude == 0) {
+                double[] facePowers =  robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude);
+                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
+            } else {
+                powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees);
+            }
+        }
+
+        robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
+
+        if (engine.gamepad1.y) {
+            double distanceToTarget = Math.hypot(robot.LAUNCH_POSITION_X - robot.getLocationX(), robot.LAUNCH_POSITION_Y - robot.getLocationY());
+            if (distanceToTarget > robot.LAUNCH_TOLERANCE_POS) {
+                powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y), POWER_NORMAL, robot.LAUNCH_ROTATION);
+
+            } else if (robot.getRelativeAngle(robot.LAUNCH_ROTATION, robot.getRotation()) > robot.LAUNCH_TOLERANCE_FACE) {
+
+            }
+        }
+
+    }
 }