Fine tuned the diamiter value for unit conversions. Added ability to disable IMUDrive and IMUTurn with the configuration tool.

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

@@ -15,6 +15,8 @@ public class IMUDrive extends CyberarmState {
     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;
@@ -30,11 +32,17 @@ public class IMUDrive extends CyberarmState {
         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.encoderRight.getCurrentPosition();
     }
 
     @Override
@@ -42,9 +50,10 @@ public class IMUDrive extends CyberarmState {
 
         robot.updateLocation();
 
-        if (Math.abs(robot.encoderRight.getCurrentPosition()) > tickTarget) {
+        int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart);
-            robot.encoderRight.setPower(0);
+        if (ticksTraveled > tickTarget) {
-            robot.encoderLeft.setPower(0);
+            robot.setDrivePower(0,0);
+            sleep(finishDelay);
             setHasFinished(true);
         } else {
 
@@ -57,8 +66,21 @@ public class IMUDrive extends CyberarmState {
 
             double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
 
-            robot.encoderRight.setPower(rightPower * powerAdjust);
+            robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower);
-            robot.encoderLeft.setPower(leftPower * powerAdjust);
         }
     }
+
+    @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

@@ -9,7 +9,7 @@ public class IMUTurn extends CyberarmState {
     private String actionName;
     private String groupName;
     private double power;
-    private double angleTarget;
+    private float angleTarget;
     private int turnDirection;
     private float angleAllowance;
     private boolean useOptimalDirection;
@@ -29,21 +29,18 @@ public class IMUTurn extends CyberarmState {
         useOptimalDirection = (turnDirection==0);
     }
 
+    @Override
+    public void start() {
+        if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
+            setHasFinished(true);
+        }
+    }
+
     @Override
     public void exec() {
         robot.updateLocation();
 
-        int OptimalDirection;
+        int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation());
-
-        if (angleTarget > 180 + robot.getRotation()){
-            OptimalDirection = -1;
-        } else if (angleTarget < -180 + robot.getRotation()){
-            OptimalDirection = 1;
-        } else if (angleTarget > robot.getRotation()){
-            OptimalDirection = 1;
-        }else {
-            OptimalDirection = -1;
-        }
 
         if (!useOptimalDirection && OptimalDirection == turnDirection){
             useOptimalDirection = true;
@@ -53,14 +50,37 @@ public class IMUTurn extends CyberarmState {
             turnDirection = OptimalDirection;
         }
 
-        robot.encoderLeft.setPower(-power * turnDirection);
+        robot.setDrivePower(power * turnDirection,-power * turnDirection);
-        robot.encoderRight.setPower(power * turnDirection);
 
         if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
-            robot.encoderRight.setPower(0);
+            robot.setDrivePower(0,0);
-            robot.encoderLeft.setPower(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

@@ -20,5 +20,8 @@ public class LocalizerTestingEngine extends CyberarmEngine {
     @Override
     public void setup() {
         addState(new IMUDrive(robot,"group", "010_drive"));
+        addState(new IMUTurn(robot,"group", "020_turn"));
+        addState(new IMUDrive(robot,"group", "030_drive"));
+        addState(new IMUTurn(robot,"group", "040_turn"));
     }
 }

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

@@ -29,7 +29,7 @@ public class Robot {
     double BIAS_LEFT = -1.0;
     double BIAS_RIGHT = -0.87;
 
-    double Circumference_Encoder = Math.PI * 3.8;
+    double Circumference_Encoder = Math.PI * 4;
     int Counts_Per_Revolution = 8192;
 
     //Robot Localizatoin
@@ -37,6 +37,9 @@ public class Robot {
     private double locationY;
     private float rotation;
 
+    public double traveledLeft;
+    public double traveledRight;
+
     private int encoderFrontPrevious = 0;
     private int encoderLeftPrevious = 0;
     private int encoderBackPrevious = 0;
@@ -50,6 +53,11 @@ public class Robot {
 //        encoderBack = hardwareMap.dcMotor.get("encoderBack");
         encoderRight = hardwareMap.dcMotor.get("encoderRight");
 
+        encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
         encoderRight.setDirection(DcMotorSimple.Direction.REVERSE);
         
         encoderLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -72,13 +80,18 @@ public class Robot {
     }
 
     public void updateLocation(){
-        rotation = imu.getAngularOrientation().firstAngle;
+        // IMU orientation is inverted to have clockwise be positive.
+        rotation = -imu.getAngularOrientation().firstAngle;
+
         float rotationChange = rotation - rotationPrevious;
         int encoderLeftCurrent = encoderLeft.getCurrentPosition();
         int encoderRightCurrent = encoderRight.getCurrentPosition();
         double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
         double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
 
+        traveledLeft += Math.abs(encoderLeftChange);
+        traveledRight += Math.abs(encoderRightChange);
+
         encoderLeftPrevious = encoderLeftCurrent;
         encoderRightPrevious = encoderRightCurrent;
         rotationPrevious = rotation;