Prepared for switch to mecanum drive train

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

@@ -0,0 +1,62 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+import java.lang.reflect.Array;
+import java.util.ArrayList;
+
+public class MechanumBiasCalibrator extends CyberarmState {
+
+    private Robot robot;
+    private ArrayList<double[]> Powers;
+    private double BiasFR;
+    private double BiasFL;
+    private double BiasBR;
+    private double BiasBL;
+    private boolean hasCalculated;
+
+
+    public MechanumBiasCalibrator(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        if (engine.gamepad1.x) {
+            double[] mecanumPowers = robot.getMecanumPowers(0, 1, 0);
+
+            //TODO: add motors when they exist.
+
+            Powers.add(mecanumPowers);
+        }
+
+        if (engine.gamepad1.y && !hasCalculated) {
+            hasCalculated = true;
+
+            double sumFR = 0;
+            double sumFL = 0;
+            double sumBR = 0;
+            double sumBL = 0;
+
+            for (double[] powers : Powers) {
+                sumFR+= powers[0];
+                sumFL+= powers[1];
+                sumBR+= powers[2];
+                sumBL+= powers[3];
+            }
+
+            int length = Powers.size();
+            BiasFR = sumFR / length;
+            BiasFL = sumFL / length;
+            BiasBR = sumBR / length;
+            BiasBL = sumBL / length;
+
+        } else if (!engine.gamepad1.y) {
+            hasCalculated = false;
+        }
+
+    }
+}

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

@@ -7,10 +7,6 @@ import org.timecrafters.UltimateGoal.Robot;
 public class EncoderTest extends CyberarmState {
 
     private Robot robot;
-    private int ticksLeft;
-    private int ticksRight;
-    private double biasLeft = 0;
-    private double biasRight = 0;
 
     public EncoderTest(Robot robot) {
         this.robot = robot;
@@ -18,51 +14,8 @@ public class EncoderTest extends CyberarmState {
 
     @Override
     public void exec() {
-        robot.updateLocation();
 
-        robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y);
+        robot.setDrivePower(1, 1,1,1);
 
-        ticksLeft= robot.encoderLeft.getCurrentPosition();
-        ticksRight=robot.encoderRight.getCurrentPosition();
-
-
-//        if (runTime() < 3000) {
-//
-//
-//
-//
-//        } else {
-//            robot.encoderLeft.setPower(0.0);
-//            robot.encoderRight.setPower(0.0);
-//
-//            double ticksExtreme;
-//            if (Math.abs(ticksLeft) < Math.abs(ticksRight)) {
-//                ticksExtreme = ticksLeft;
-//            } else {
-//                ticksExtreme = ticksRight;
-//            }
-//            biasLeft = ticksExtreme/ticksLeft;
-//            biasRight = ticksExtreme/ticksRight;
-//        }
-
-    }
-
-    @Override
-    public void telemetry() {
-
-        engine.telemetry.addData("controller", engine.gamepad1.left_stick_y);
-        engine.telemetry.addLine("Latency 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("Actual Values");
-        engine.telemetry.addData("Left", robot.ticksToInches(ticksLeft));
-        engine.telemetry.addData("Right", robot.ticksToInches(ticksRight));
-
-//        engine.telemetry.addLine("");
-//        engine.telemetry.addData("Front", robot.encoderFront);
-//        engine.telemetry.addData("Back", robot.encoderBack);
     }
 }

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

@@ -0,0 +1,87 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+
+public class MecanumFunctionTest extends CyberarmState {
+
+    private Robot robot;
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double powerFrontLeft;
+    private double powerFrontRight;
+    private double powerBackLeft;
+    private double powerBackRight;
+
+    private static double TURN_POWER_SCALE = 0.5;
+
+    public MecanumFunctionTest(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.left_stick_x;
+        double rightJoystickY = engine.gamepad1.left_stick_y;
+
+        rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
+        rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+        powerFrontLeft = 0;
+        powerFrontRight = 0;
+        powerBackLeft = 0;
+        powerBackRight = 0;
+
+        if (rightJoystickMagnitude == 0) {
+            double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees);
+            powerFrontLeft = powers[0];
+            powerFrontRight = powers[1];
+            powerBackLeft = powers[2];
+            powerBackRight = powers[3];
+        } else {
+            if (leftJoystickMagnitude == 0) {
+                double[] powers =  robot.getFacePowers(rightJoystickDegrees, rightJoystickMagnitude);
+                powerFrontLeft = TURN_POWER_SCALE * powers[0];
+                powerFrontRight = TURN_POWER_SCALE * powers[1];
+                powerBackLeft = TURN_POWER_SCALE * powers[0];
+                powerBackRight = TURN_POWER_SCALE * powers[1];
+            } else {
+                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, rightJoystickDegrees);
+                powerFrontLeft = powers[0];
+                powerFrontRight = powers[1];
+                powerBackLeft = powers[2];
+                powerBackRight = powers[3];
+            }
+        }
+
+        robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
+
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addLine("Left Joystick");
+        engine.telemetry.addData("Angle", leftJoystickDegrees);
+        engine.telemetry.addData("Mag", leftJoystickMagnitude);
+
+        engine.telemetry.addLine("Right Joystick");
+        engine.telemetry.addData("Angle", leftJoystickDegrees);
+        engine.telemetry.addData("Mag", leftJoystickMagnitude);
+
+//        engine.telemetry.addLine("");
+//        engine.telemetry.addData("Front", robot.encoderFront);
+//        engine.telemetry.addData("Back", robot.encoderBack);
+    }
+}

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

@@ -5,24 +5,27 @@ 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 = "Mecanum test", group = "test")
 public class TestingEngine extends CyberarmEngine {
 
     private Robot robot;
 
-//    @Override
+    @Override
-//    public void init() {
+    public void init() {
-//        robot = new Robot(hardwareMap);
+        robot = new Robot(hardwareMap);
-//        robot.initHardware();
+        robot.initHardware();
-//        super.init();
+        super.init();
-//    }
+    }
 
 
 
     @Override
     public void setup() {
-        addState(new WelcomeToJankyTown());
+        addState(new EncoderTest(robot));
     }
 
-
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+    }
 }

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

@@ -48,7 +48,7 @@ public class DriveToPosition extends CyberarmState {
 //            }
 
         if (distanceRemaining < tolerance) {
-            robot.setDrivePower(0, 0);
+            robot.setDrivePower(0, 0, 0, 0);
             setHasFinished(true);
         } else {
             robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);

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

@@ -1,9 +1,6 @@
 package org.timecrafters.UltimateGoal.LocalizerTesting;
 
-import android.util.Log;
-
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.TimeCraftersConfigurationTool.backend.Backend;
 import org.timecrafters.UltimateGoal.Robot;
 
 public class IMUDrive extends CyberarmState {
@@ -41,7 +38,7 @@ public class IMUDrive extends CyberarmState {
         }
 
         angleTarget=robot.getRotation();
-        tickStart = robot.encoderRight.getCurrentPosition();
+        tickStart = robot.driveFrontRight.getCurrentPosition();
     }
 
     @Override
@@ -49,7 +46,7 @@ public class IMUDrive extends CyberarmState {
 
         robot.updateLocation();
 
-        int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart);
+        int ticksTraveled = Math.abs( robot.driveFrontRight.getCurrentPosition()-tickStart);
         if (ticksTraveled > tickTarget) {
             robot.setDrivePower(0,0);
             sleep(finishDelay);

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

@@ -1,6 +1,5 @@
 package org.timecrafters.UltimateGoal;
 
-import android.app.Activity;
 import android.os.Environment;
 import android.util.Log;
 
@@ -14,15 +13,12 @@ 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.VuforiaBase;
 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 org.timecrafters.TimeCraftersConfigurationTool.backend.Backend;
-import org.timecrafters.TimeCraftersConfigurationTool.backend.TAC;
 
 import java.io.File;
 import java.io.FileWriter;
@@ -50,13 +46,16 @@ public class   Robot {
     private VuforiaLocalizer vuforia;
 
     //drive system
-    public DcMotor encoderFront;
-    public DcMotor encoderLeft;
-    public DcMotor encoderBack;
-    public DcMotor encoderRight;
 
-    static final double BIAS_LEFT = -1.0;
+    public DcMotor driveBackLeft;
-    static final double BIAS_RIGHT = -0.87;
+    public DcMotor driveFrontLeft;
+    public DcMotor driveBackRight;
+    public DcMotor driveFrontRight;
+
+    static final double BIAS_FRONT_LEFT = 1;
+    static final double BIAS_FRONT_RIGHT = 1;
+    static final double BIAS_BACK_LEFT = 1;
+    static final double BIAS_BACK_RIGHT = 1;
 
     //Conversion Constants
     static final double ENCODER_CIRCUMFERENCE = Math.PI * 4;
@@ -84,7 +83,7 @@ public class   Robot {
     public double traveledLeft;
     public double traveledRight;
 
-    private int encoderFrontPrevious = 0;
+
     private int encoderLeftPrevious = 0;
     private int encoderBackPrevious = 0;
     private int encoderRightPrevious = 0;
@@ -105,20 +104,21 @@ public class   Robot {
         webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
         imu  = hardwareMap.get(BNO055IMU.class, "imu");
-//        encoderFront = hardwareMap.dcMotor.get("encoderFront");
-        encoderLeft = hardwareMap.dcMotor.get("encoderLeft");
-//        encoderBack = hardwareMap.dcMotor.get("encoderBack");
-        encoderRight = hardwareMap.dcMotor.get("encoderRight");
 
-        encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
-        encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
-        encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
-        encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
 
-        encoderRight.setDirection(DcMotorSimple.Direction.REVERSE);
+        driveFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        driveFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        driveFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        driveFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
         
-        encoderLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        encoderRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
@@ -184,7 +184,9 @@ public class   Robot {
         targetsUltimateGoal.activate();
     }
 
-    //TODO : Test range of Tensor Object identification.
+    public void deactivateVuforia() {
+        targetsUltimateGoal.deactivate();
+    }
 
     private void initTensorFlow() {
         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
@@ -195,12 +197,6 @@ public class   Robot {
         tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
     }
 
-    public void setDrivePower(double powerLeft, double powerRight){
-        encoderLeft.setPower(powerLeft * BIAS_LEFT);
-        encoderRight.setPower(powerRight * BIAS_RIGHT);
-
-    }
-
     public void updateLocation(){
         // orientation is inverted to have clockwise be positive.
         float imuAngle = -imu.getAngularOrientation().firstAngle;
@@ -213,8 +209,8 @@ public class   Robot {
             rotationChange += 360;
         }
 
-        int encoderLeftCurrent = encoderLeft.getCurrentPosition();
+        int encoderLeftCurrent = driveFrontLeft.getCurrentPosition();
-        int encoderRightCurrent = encoderRight.getCurrentPosition();
+        int encoderRightCurrent = driveFrontRight.getCurrentPosition();
         double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
         double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
 
@@ -327,6 +323,65 @@ public class   Robot {
         return relative;
     }
 
+    //Drive Functions
+    public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){
+        driveFrontLeft.setPower(powerFrontLeft * BIAS_FRONT_LEFT);
+        driveFrontRight.setPower(powerFrontLeft * BIAS_FRONT_RIGHT);
+        driveBackLeft.setPower(powerBackLeft * BIAS_BACK_LEFT);
+        driveBackLeft.setPower(powerBackRight * BIAS_BACK_RIGHT);
+    }
+
+    public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) {
+        double rad = Math.toRadians(degreesDirectionMotion);
+        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;
+
+        float relativeRotation =  getRelativeAngle(degreesDirectionFace, rotation);
+        double turnCorrection = Math.pow(0.03 * relativeRotation, 3) + 0.02 * relativeRotation;
+
+        double powerForwardRight = q - turnCorrection;
+        double powerForwardLeft = p + turnCorrection;
+        double powerBackRight = p - turnCorrection;
+        double powerBackLeft = 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};
+
+        return powers;
+    }
+
+    public double[] getFacePowers(float direction, double power) {
+        double left = power;
+        double right = -power;
+
+        if (getRelativeAngle(direction, rotation) > 0) {
+            left *= -1;
+            right *= -1;
+        }
+        double[] powers = {left,right};
+        return powers;
+    }
+
+    //This function should not be used
     public void driveAtAngle(float angle, double power) {
 
         double relativeAngle = getRelativeAngle(angle, getRotation());
@@ -347,9 +402,10 @@ public class   Robot {
         // maintaining the power ratio nesesary to execute the turn.
         double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
 
-        setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust);
+//        setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust);
     }
 
+    //Data Recording
     public void record() {
         TestingRecord+="\n"+locationX+","+locationY+","+rotation;
     }
@@ -373,7 +429,5 @@ public class   Robot {
         }
     }
 
-    public void deactivateVuforia() {
+
-        targetsUltimateGoal.deactivate();
-    }
 }