So many things

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

@@ -1,4 +0,0 @@
-package org.timecrafters.UltimateGoal;
-
-public class AutoEngine {
-}

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

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

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

@@ -1,9 +1,8 @@
 package org.timecrafters.UltimateGoal.Calibration;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
-import java.lang.reflect.Array;
 import java.util.ArrayList;
 
 public class MechanumBiasCalibrator extends CyberarmState {

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

@@ -1,7 +1,7 @@
 package org.timecrafters.UltimateGoal.Calibration;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 public class OdometryCalibration extends CyberarmState {
 

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

@@ -1,9 +1,7 @@
 package org.timecrafters.UltimateGoal.Calibration;
 
-import com.qualcomm.robotcore.hardware.DcMotor;
-
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 import java.util.ArrayList;
 

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

@@ -3,9 +3,7 @@ package org.timecrafters.UltimateGoal.Calibration;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
-
-import java.util.ArrayList;
 
 public class StalPowerCalibrator extends CyberarmState {
 

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

@@ -1,11 +1,8 @@
 package org.timecrafters.UltimateGoal.Calibration;
 
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
-
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.DriveMotor;
+import org.timecrafters.UltimateGoal.Competition.DriveMotor;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 public class VelocityTest extends CyberarmState {
 

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

@@ -0,0 +1,4 @@
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
+
+public class AutoEngine {
+}

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

@@ -1,7 +1,7 @@
-package org.timecrafters.UltimateGoal.Autonomous;
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 public class DriveToCoordinates extends CyberarmState {
 

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

@@ -1,7 +1,7 @@
-package org.timecrafters.UltimateGoal.LocalizerTesting;
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 public class DriveToPosition extends CyberarmState {
 
@@ -51,7 +51,7 @@ public class DriveToPosition extends CyberarmState {
             robot.setDrivePower(0, 0, 0, 0);
             setHasFinished(true);
         } else {
-            robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);
+//            robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);
         }
 
     }

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

@@ -1,4 +1,4 @@
-package org.timecrafters.UltimateGoal.Autonomous;
+package org.timecrafters.UltimateGoal.Competition.Autonomous;
 
 import org.cyberarm.engine.V2.CyberarmState;
 

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

@@ -1,4 +1,4 @@
-package org.timecrafters.UltimateGoal;
+package org.timecrafters.UltimateGoal.Competition;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;

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

@@ -0,0 +1,4 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+public class Launch {
+}

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

@@ -1,4 +1,4 @@
-package org.timecrafters.UltimateGoal;
+package org.timecrafters.UltimateGoal.Competition;
 
 import android.os.Environment;
 import android.util.Log;
@@ -6,6 +6,7 @@ import android.util.Log;
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
@@ -59,8 +60,6 @@ public class   Robot {
     public DcMotor encoderRight;
     public DcMotor encoderBack;
 
-    public DcMotor launcher;
-
     //Steering Constants
     static final double FINE_CORRECTION = 0.05 ;
     static final double LARGE_CORRECTION = 0.002;
@@ -97,6 +96,7 @@ public class   Robot {
     //Launcher
     public DcMotor launchMotor;
 
+    private static final long LAUNCH_ACCEL_TIME = 3000;
     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;
@@ -104,10 +104,18 @@ public class   Robot {
     public static final double LAUNCH_TOLERANCE_FACE = 0.5;
 
     //Ring Belt
+    public DcMotor collectionMotor;
+    public DcMotor ringBeltMotor;
+    private DigitalChannel limitSwitch;
+
+    //todo: tune these when they exist
     public static final int RING_BELT_LOOP_TICKS = 1000;
-    public static final int RING_BELT_POS_1 = 200;
+    public static final int RING_BELT_GAP = 200;
-    public static final int RING_BELT_POS_2 = 400;
+    public static final int RING_BELT_RECEIVE_POS = 100;
-    public static final int RING_BELT_POS_3 = 600;
+
+    public static final double RING_DETECT_DISTANCE = 100;
+    public static final double RING_DETECT_DELAY = 1000;
+
 
     //Debugging
     public double visionX;
@@ -139,6 +147,7 @@ public class   Robot {
 //        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
         imu  = hardwareMap.get(BNO055IMU.class, "imu");
+//        limitSwitch.setMode(DigitalChannel.Mode.INPUT);
 
         DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft");
 
@@ -157,12 +166,14 @@ public class   Robot {
         driveBackLeft.init();
         driveBackRight.init();
 
-        //todo add these when they exist
+//        encoderLeft = hardwareMap.dcMotor.get("odoLeft");
-        encoderLeft = hardwareMap.dcMotor.get("odoLeft");
 //        encoderRight = hardwareMap.dcMotor.get("odoRight");
 //        encoderBack = hardwareMap.dcMotor.get("odoBack");
-//
+
-//        launcher = hardwareMap.dcMotor.get("launcher");
+        launchMotor = hardwareMap.dcMotor.get("launcher");
+        collectionMotor = hardwareMap.dcMotor.get("collect");
+        collectionMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        ringBeltMotor = hardwareMap.dcMotor.get("belt");
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
@@ -190,6 +201,14 @@ public class   Robot {
         rotation = stateConfiguration.variable("system", "startPos", "direction").value();
         locationX = stateConfiguration.variable("system", "startPos", "x").value();
         locationY = stateConfiguration.variable("system", "startPos", "y").value();
+
+//        double launcherPower = 0;
+//        long launchAccelStart = System.currentTimeMillis();
+//        while (launcherPower < 1) {
+//            launcherPower = (double) (System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME;
+//            launchMotor.setPower(launcherPower);
+//        }
+
     }
 
     private void initVuforia() {
@@ -549,34 +568,17 @@ public class   Robot {
         return powers;
     }
 
-    //This function should not be used
+    public int getBeltPos(){
-    public void driveAtAngle(float angle, double power) {
+        return loopPos(ringBeltMotor.getCurrentPosition());
-
-        double relativeAngle = getRelativeAngle(angle, getRotation());
-
-        //calculate how the power of each motor should be adjusted to make the robot curve
-        //towards the target angle
-        //--------------------------------------------------------------------------------------
-
-        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;
-        double rightPower = power + turnPowerCorrection;
-        //--------------------------------------------------------------------------------------
-
-
-        //calculates speed adjuster that slows the motors to be closer to the BasePower while
-        // maintaining the power ratio nesesary to execute the turn.
-        double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
-
-//        setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust);
     }
 
-    //Data Recording
+    public int loopPos(int pos) {
-//    public void record(double frontLeft, double frontRight, double backLeft, double backRight) {
+        if (pos < 0) {
-//        TestingRecord+="\n"+frontLeft+","+frontRight+","+backLeft+","+backRight;
+         pos += RING_BELT_LOOP_TICKS;
-//    }
+        }
+        pos %= RING_BELT_LOOP_TICKS;
+        return pos;
+    }
 
     public void record() {
         TestingRecord+="\n"+angularVelocity;

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

@@ -1,4 +1,4 @@
-package org.timecrafters.UltimateGoal;
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 

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

@@ -1,7 +1,8 @@
-package org.timecrafters.UltimateGoal;
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 public class TeleOpState extends CyberarmState {
 
@@ -16,13 +17,20 @@ public class TeleOpState extends CyberarmState {
     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 start() {
+
+    }
+
     @Override
     public void exec() {
         robot.updateLocation();
@@ -41,43 +49,44 @@ public class TeleOpState extends CyberarmState {
 
         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) {
+                //todo add launch sequence
 
             }
+        } else {
+            boolean joystickButton = engine.gamepad1.left_stick_button;
+
+            if (joystickButton && !toggleSpeedInput) {
+                if (powerScale == POWER_NORMAL) {
+                    powerScale = POWER_SPRINT;
+                } else {
+                    powerScale = POWER_NORMAL;
+                }
+            }
+
+            toggleSpeedInput = joystickButton;
+
+
+            if (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]);
+
     }
 }

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

@@ -1,12 +1,10 @@
 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;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 
 public class ControlHubTest extends CyberarmState {

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

@@ -0,0 +1,37 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.robotcore.hardware.ColorRangeSensor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+
+public class DistanceSensorTest extends CyberarmState {
+
+
+    private Rev2mDistanceSensor distanceSensor;
+//    private RevColorSensorV3 colorSensor;
+    private ColorRangeSensor colorSensor;
+    private double distance = 0;
+    private double color = 0;
+
+    @Override
+    public void init() {
+        distanceSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distance");
+        colorSensor = engine.hardwareMap.get(ColorRangeSensor.class, "color");
+//        colorSensor = engine.hardwareMap.get(RevColorSensorV3.class, "color");
+    }
+
+    @Override
+    public void exec() {
+        distance = distanceSensor.getDistance(DistanceUnit.MM);
+        color = colorSensor.getDistance(DistanceUnit.MM);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("distance", distance);
+        engine.telemetry.addData("color", color);
+    }
+}

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

@@ -1,20 +0,0 @@
-package org.timecrafters.UltimateGoal.HardwareTesting;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
-
-
-public class EncoderTest extends CyberarmState {
-
-    private Robot robot;
-
-    public EncoderTest(Robot robot) {
-        this.robot = robot;
-    }
-
-
-    @Override
-    public void exec() {
-
-    }
-}

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

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

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

@@ -4,7 +4,7 @@ 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;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
 
 public class MecanumFunctionTest extends CyberarmState {

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

@@ -3,13 +3,12 @@ package org.timecrafters.UltimateGoal.HardwareTesting;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.UltimateGoal.Robot;
+import org.timecrafters.UltimateGoal.Competition.Robot;
 
-@TeleOp (name = "Performance test", group = "test")
+@TeleOp (name = "Hardware test", group = "test")
 public class TestingEngine extends CyberarmEngine {
 
     private Robot robot;
-
     @Override
     public void init() {
         robot = new Robot(hardwareMap);
@@ -17,16 +16,14 @@ public class TestingEngine extends CyberarmEngine {
         super.init();
     }
 
-
-
     @Override
     public void setup() {
-        addState(new MecanumFunctionTest(robot));
+        addState(new FullTest(robot));
-    }
-
-    @Override
-    public void stop() {
-        robot.saveRecording();
-        super.stop();
     }
+//
+//    @Override
+//    public void stop() {
+//        robot.saveRecording();
+//        super.stop();
+//    }
 }

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

@@ -1,82 +0,0 @@
-package org.timecrafters.UltimateGoal.LocalizerTesting;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
-
-public class IMUDrive extends CyberarmState {
-
-    private Robot robot;
-    private String actionName;
-    private String groupName;
-    private double power;
-    private int tickTarget;
-    private float angleRelative;
-    private float angleTarget;
-    private int tickStart;
-    private long finishDelay;
-
-    public IMUDrive(Robot robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.actionName = actionName;
-        this.groupName = groupName;
-    }
-
-    @Override
-    public void init() {
-
-        power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
-        double inchesTarget = robot.stateConfiguration.variable(groupName, actionName, "inches").value();
-        tickTarget = (int) robot.inchesToTicks(inchesTarget);
-        angleTarget = robot.stateConfiguration.variable(groupName, actionName, "angle").value();
-        finishDelay = robot.stateConfiguration.variable(groupName, actionName, "delay").value();
-    }
-
-    @Override
-    public void start() {
-        if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
-            setHasFinished(true);
-        }
-
-        angleTarget=robot.getRotation();
-        tickStart = robot.driveFrontRight.motor.getCurrentPosition();
-    }
-
-    @Override
-    public void exec() {
-
-        robot.updateLocation();
-
-        int ticksTraveled = Math.abs( robot.driveFrontRight.motor.getCurrentPosition()-tickStart);
-        if (ticksTraveled > tickTarget) {
-//            robot.setDrivePower(0,0);
-            sleep(finishDelay);
-            setHasFinished(true);
-        } else {
-
-            angleRelative = robot.getRelativeAngle(angleTarget, robot.getRotation());
-
-            double turnPowerCorrection = Math.pow(0.03 * angleRelative, 3) + 0.01 * angleRelative;
-
-            double rightPower = power + turnPowerCorrection;
-            double leftPower = power - turnPowerCorrection;
-
-            double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
-
-//            robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower);
-        }
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addLine("Measured Values");
-        engine.telemetry.addData("Y", robot.ticksToInches(robot.getLocationY()));
-        engine.telemetry.addData("X", robot.ticksToInches(robot.getLocationX()));
-        engine.telemetry.addLine();
-        engine.telemetry.addData("Rotation", robot.getRotation());
-        engine.telemetry.addLine();
-        engine.telemetry.addLine("Total Travel");
-        engine.telemetry.addData("Left", robot.ticksToInches(robot.traveledLeft));
-        engine.telemetry.addData("Right", robot.ticksToInches(robot.traveledRight));
-
-    }
-}

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

@@ -1,90 +0,0 @@
-package org.timecrafters.UltimateGoal.LocalizerTesting;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
-
-public class IMUTurn extends CyberarmState {
-
-    private Robot robot;
-    private String actionName;
-    private String groupName;
-    private double power;
-    private float angleTarget;
-    private int turnDirection;
-    private float angleAllowance;
-    private boolean useOptimalDirection;
-
-    public IMUTurn(Robot robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.actionName = actionName;
-        this.groupName = groupName;
-    }
-
-    @Override
-    public void init() {
-        power = robot.stateConfiguration.variable(groupName,actionName, "power").value();
-        angleTarget = robot.stateConfiguration.variable(groupName,actionName, "angle").value();
-        angleAllowance = robot.stateConfiguration.variable(groupName,actionName, "allowance").value();
-
-        // turnDirection = robot.stateConfiguration.variable(groupName, actionName, "direction").value();
-
-        turnDirection = 0;
-
-        useOptimalDirection = (turnDirection == 0);
-    }
-
-    @Override
-    public void start() {
-        if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
-            setHasFinished(true);
-        }
-    }
-
-    @Override
-    public void exec() {
-        robot.updateLocation();
-
-        int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation());
-
-        if (!useOptimalDirection && OptimalDirection == turnDirection){
-            useOptimalDirection = true;
-        }
-
-        if (useOptimalDirection){
-            turnDirection = OptimalDirection;
-        }
-
-//        robot.setDrivePower(power * turnDirection,-power * turnDirection);
-
-        if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
-//            robot.setDrivePower(0,0);
-            setHasFinished(true);
-        }
-
-
-
-    }
-
-    private int getOptimalDirection(float angleTarget, float angleCurrent){
-
-        if (angleCurrent < 0) {
-            angleCurrent += 360;
-        }
-
-        if (angleTarget < 0) {
-            angleTarget += 360;
-        }
-
-        float degreeDifferance = angleTarget - angleCurrent;
-        if (degreeDifferance > 180 || (degreeDifferance < 0 && degreeDifferance > -180)) {
-            return -1;
-        } else {
-            return 1;
-        }
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("rotation", robot.getRotation());
-    }
-}

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

@@ -1,35 +0,0 @@
-package org.timecrafters.UltimateGoal.LocalizerTesting;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.UltimateGoal.Robot;
-
-@TeleOp(name = "Localizer Test")
-public class LocalizerTestingEngine extends CyberarmEngine {
-
-    private Robot robot;
-
-    @Override
-    public void init() {
-        robot = new Robot(hardwareMap);
-        robot.initHardware();
-        super.init();
-    }
-
-    @Override
-    public void setup() {
-        addState(new VuforiaNavTesting(robot));
-//        addState(new DriveToPosition(robot, "DtoP", "d1"));
-//        addState(new DriveToPosition(robot, "DtoP", "d2"));
-
-    }
-
-    @Override
-    public void stop() {
-        super.stop();
-        robot.saveRecording();
-        robot.deactivateVuforia();
-    }
-}

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

@@ -1,58 +0,0 @@
-package org.timecrafters.UltimateGoal.LocalizerTesting;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.UltimateGoal.Robot;
-
-
-public class VuforiaNavTesting extends CyberarmState {
-
-    private Robot robot;
-    private float angle;
-    private double X;
-    private double Y;
-    private float joystickDegrees;
-    private double power;
-
-    public VuforiaNavTesting(Robot robot) {
-        this.robot = robot;
-    }
-
-    @Override
-    public void exec() {
-        if (engine.gamepad1.x) {
-            robot.syncWithVuforia();
-        }
-        
-        robot.updateLocation();
-//        robot.record();
-
-        double joystickX = engine.gamepad1.right_stick_x;
-        double joystickY = engine.gamepad1.right_stick_y;
-
-        joystickDegrees = (float) Math.toDegrees(Math.atan2(joystickX, -joystickY));
-        power = 0.3 * Math.hypot(joystickX, joystickY);
-
-        robot.driveAtAngle(joystickDegrees, power);
-
-        X = robot.getLocationX();
-        Y = robot.getLocationY();
-        angle = robot.getRotation();
-
-
-
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("Target Visible", robot.trackableVisible);
-        engine.telemetry.addData("Odo X", robot.ticksToInches(X));
-        engine.telemetry.addData("Odo Y", robot.ticksToInches(Y));
-
-        engine.telemetry.addData("Joystick", joystickDegrees);
-//        engine.telemetry.addData("Vis X", robot.ticksToInches(robot.visionX));
-//        engine.telemetry.addData("Vis Y", robot.ticksToInches(robot.visionY));
-
-        engine.telemetry.addData("Robot Angle", angle);
-    }
-
-}