Reorganized drive motors, added baking, and RUN_USING_ENCODERS. changed robot localization to work with new odometry designs and added OdometryCalibration

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

@@ -21,6 +21,6 @@ public class CalibrationEngine extends CyberarmEngine {
 
     @Override
     public void setup() {
-        addState(new MechanumBiasCalibrator(robot));
+        addState(new VelocityTest(robot));
     }
 }

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

@@ -0,0 +1,39 @@
+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;
+
+    public OdometryCalibration(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+
+        if (engine.gamepad1.x) {
+            double power = 0.1;
+            robot.setDrivePower(power, -power, power, -power);
+            currentTick = robot.encoderBack.getCurrentPosition();
+            rotation = -robot.imu.getAngularOrientation().firstAngle;
+            ticksPerDegree = 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);
+    }
+}

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

@@ -28,7 +28,7 @@ public class StalPowerCalibrator extends CyberarmState {
 
         for (DcMotor motor : Motors) {
             motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         }
     }
 
@@ -51,7 +51,7 @@ public class StalPowerCalibrator extends CyberarmState {
                 sleep(1000);
                 for (DcMotor motor : Motors) {
                     motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-                    motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                    motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
                 }
             }
         }

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

@@ -0,0 +1,52 @@
+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.Robot;
+
+public class VelocityTest extends CyberarmState {
+
+    private Robot robot;
+    private double[] Velocities;
+    private DriveMotor[] Motors;
+    private long LastTime = 0;
+
+
+
+    public  VelocityTest(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        Motors = new DriveMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
+        Velocities = new double[] {0,0,0,0};
+
+    }
+
+    @Override
+    public void start() {
+        for (DriveMotor motor : Motors) {
+            motor.setPower(0.1);
+        }
+    }
+
+    @Override
+    public void exec() {
+
+        for (int i = 0; i < 4; i++) {
+            Velocities[i] = (Motors[i].motor.getCurrentPosition()* 60000) / runTime();
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("FrontLeft", Velocities[0]);
+        engine.telemetry.addData("FrontRight", Velocities[1]);
+        engine.telemetry.addData("BackLeft", Velocities[2]);
+        engine.telemetry.addData("BackRight", Velocities[3]);
+    }
+}

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

@@ -0,0 +1,45 @@
+package org.timecrafters.UltimateGoal;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+public class DriveMotor {
+
+    public DcMotor motor;
+    private int brakePosition;
+    private MotorConfigurationType motorType;
+    private DcMotorSimple.Direction direction;
+    static final double FINE_CORRECTION = 0.02;
+    static final double LARGE_CORRECTION = 0.03;
+
+    public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) {
+        this.motor = motor;
+        this.motorType = motorType;
+        this.direction = direction;
+    }
+
+    public void init() {
+        motor.setMotorType(motorType);
+        motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        motor.setDirection(direction);
+    }
+
+    public void setPower(double power) {
+        motor.setPower(power);
+    }
+
+    public void setBrakePosition() {
+        brakePosition = motor.getCurrentPosition();
+    }
+
+    public void brake() {
+        double currentPosition = motor.getCurrentPosition();
+        double relativePosition = brakePosition - currentPosition;
+        double breakPower = Math.pow(LARGE_CORRECTION * relativePosition, 3) + FINE_CORRECTION * relativePosition;
+
+
+    }
+
+}

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

@@ -7,6 +7,7 @@ import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
 import org.firstinspires.ftc.robotcore.external.ClassFactory;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -47,10 +48,19 @@ public class   Robot {
 
     //drive system
 
-    public DcMotor driveBackLeft;
+    public DriveMotor driveFrontLeft;
-    public DcMotor driveFrontLeft;
+    public DriveMotor driveBackLeft;
-    public DcMotor driveBackRight;
+    public DriveMotor driveFrontRight;
-    public DcMotor driveFrontRight;
+    public DriveMotor driveBackRight;
+
+    public DcMotor encoderLeft;
+    public DcMotor encoderRight;
+    public DcMotor encoderBack;
+
+    private int brakePosFrontLeft;
+    private int brakePosFrontRight;
+    private int brakePosBackLeft;
+    private int brakePosBackRight;
 
     static final double BIAS_FRONT_LEFT = 1;
     static final double BIAS_FRONT_RIGHT = 1;
@@ -58,12 +68,14 @@ public class   Robot {
     static final double BIAS_BACK_RIGHT = 1;
 
     static final double FINE_CORRECTION = 0.01;
-    static final double LARGE_CORRECTION = 0.01 ;
+    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;
 
     // Inches Forward of axis of rotation
     static final float CAMERA_FORWARD_DISPLACEMENT  = 13f;
@@ -109,18 +121,21 @@ public class   Robot {
 
         imu  = hardwareMap.get(BNO055IMU.class, "imu");
 
-        driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
+        DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft");
-        driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
-        driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
-        driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
 
-        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+        MotorConfigurationType motorType = dcMotor.getMotorType();
-        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+        motorType.setGearing(5);
+        motorType.setTicksPerRev(140);
+        motorType.setMaxRPM(1200);
 
-        driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveFrontLeft = new DriveMotor(dcMotor, motorType, DcMotorSimple.Direction.FORWARD);
-        driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveFrontRight = new DriveMotor(hardwareMap.dcMotor.get("driveFrontRight"), motorType, DcMotorSimple.Direction.REVERSE);
-        driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveBackLeft = new DriveMotor(hardwareMap.dcMotor.get("driveBackLeft"), motorType, DcMotorSimple.Direction.FORWARD);
-        driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveBackRight = new DriveMotor(hardwareMap.dcMotor.get("driveBackRight"), motorType, DcMotorSimple.Direction.REVERSE);
+
+        encoderLeft = hardwareMap.dcMotor.get("odoLeft");
+        encoderRight = hardwareMap.dcMotor.get("odoRight");
+        encoderBack = hardwareMap.dcMotor.get("odoBack");
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
@@ -203,35 +218,40 @@ public class   Robot {
     public void updateLocation(){
         // orientation is inverted to have clockwise be positive.
         float imuAngle = -imu.getAngularOrientation().firstAngle;
-        double rotationChange = imuAngle - rotationPrevious;
+        double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
 
-        if (rotationChange > 180) {
+        int encoderLeftCurrent = encoderLeft.getCurrentPosition();
-            rotationChange -= 360;
+        int encoderRightCurrent = encoderRight.getCurrentPosition();
-        }
+        int encoderBackCurrent = encoderBack.getCurrentPosition();
-        if (rotationChange < -180) {
-            rotationChange += 360;
-        }
 
-        int encoderLeftCurrent = driveFrontLeft.getCurrentPosition();
-        int encoderRightCurrent = driveFrontRight.getCurrentPosition();
         double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
         double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
-
+        double encoderBackChange = encoderBackCurrent - encoderBackPrevious;
-        traveledLeft += Math.abs(encoderLeftChange);
-        traveledRight += Math.abs(encoderRightChange);
-        rotation += rotationChange;
 
         encoderLeftPrevious = encoderLeftCurrent;
         encoderRightPrevious = encoderRightCurrent;
+        encoderBackPrevious = encoderBackCurrent;
         rotationPrevious = imuAngle;
 
-        double average = (encoderLeftChange+encoderRightChange)/2;
+        //The forward Vector has the luxury of having an odometer on both sides of the robot.
+        //This allows us to eliminate the unwanted influence of turning the robot by averaging
+        //the two. This meathod doesn't require any prior calibration.
+        double forwardVector = (encoderLeftChange+encoderRightChange)/2;
 
-        double xChange = average * (Math.sin(Math.toRadians(rotation)));
+        //Since there isn't a second wheel to remove the influence of robot rotation, we have to
-        double yChange = average * (Math.cos(Math.toRadians(rotation)));
+        //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 magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
+        double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
+
+        double xChange = magnitude * (Math.sin(direction));
+        double yChange = magnitude * (Math.cos(direction));
 
         locationX += xChange;
         locationY += yChange;
+        rotation += rotationChange;
 
 
         if (rotation > 180) {
@@ -349,7 +369,6 @@ public class   Robot {
         double y = scalar * Math.cos(rad);
         double x = scalar * Math.sin(rad);
 
-        //TODO: Try swapping p and q if left and right seam incorrect
         double p = y + x;
         double q = y - x;
 
@@ -382,7 +401,22 @@ public class   Robot {
         return powers;
     }
 
-    //
+    public void brakeAll() {
+        driveFrontLeft.brake();
+        driveFrontRight.brake();
+        driveBackLeft.brake();
+        driveBackRight.brake();
+    }
+
+    //sets the position the motors will lock to when braking to the motor's current position.
+    public void setAllBrakePos() {
+        driveFrontLeft.setBrakePosition();
+        driveFrontRight.setBrakePosition();
+        driveBackLeft.setBrakePosition();
+        driveBackRight.setBrakePosition();
+    }
+
+    //Outputs the power necessary to turn and face a provided direction
     public double[] getFacePowers(float direction, double power) {
         double relativeAngle = getRelativeAngle(direction, rotation);
         double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle;