Add unit tests to auto main

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java

@@ -3,10 +3,8 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
-import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
 import org.firstinspires.ftc.teamcode.Utils_13233.RampControls;
 import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
 
@@ -39,6 +37,7 @@ public void runOpMode() {
             telemetry.update();
 
             // Set the power to the launch motors based while the x button is being pressed
+
             launch.setLaunchPower(gamepad1.x);
 
             //Add option to enable brakes when sharbell holds a

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+public class AutoConstants {
+    // Global Variables
+    public final static double noPower = 0.0;
+    public final static double quarterPower = 0.25;
+    public final static double oneThirdPower = 0.34;
+    public final static double halfPower = 0.5;
+    public final static double threeQuartPower = 0.75;
+    public final static double fullPower = 1.0;
+
+
+    // Variables for imu
+    protected double globalAngle, correction;
+    protected YawPitchRollAngles lastAngles;
+
+    protected double voltage = 0;
+    protected static double batteryConst = 13.5;
+
+    // Variables for driving with Encoders
+    protected static final double wheelCircumference = 4.0 * Math.PI;
+    protected static final double gearRatio = 18.9;                  // Rev HD Hex 20:1
+    protected static final double countsPerRotation = 560.0;         // Rev HD Hex 20:1
+    protected static final double scaleFactor = 9.0;                 // need to find scale factor!!!
+    protected static final double countsPerInch = countsPerRotation / wheelCircumference
+        / gearRatio * scaleFactor;
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java

@@ -0,0 +1,109 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+
+import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+
+public class AutoDrive {
+    private final MotorConstructor motors;
+    CommonAutoMethods autoMethods;
+    private final LinearOpMode opMode;
+
+    public AutoDrive(LinearOpMode opMode, HardwareMap hardwareMap) {
+        this.opMode = opMode;
+        this.motors = new MotorConstructor(hardwareMap);
+        this.autoMethods = new CommonAutoMethods(opMode, hardwareMap);
+    }
+
+
+    /**
+     * Function: driveForward
+     * <p>
+     * This function is called to have the robot move forward.
+     * The robot speed is passed in and that value is used for
+     * all wheels.
+     */
+    public void driveForward(double inches, double power) {
+        driveInches(inches, power, power, power, power);
+    }
+
+
+    /**
+     * Function: driveBackward
+     * <p>
+     * This function is called to have the robot move in reverse.
+     * The robot speed is passed in and that value is used for
+     * all wheels.
+     */
+    private void driveBackward(double inches, double power) {
+        driveInches(inches, -power, -power, -power, -power);
+    }
+
+    /**
+     * Function: strafeLeft
+     * <p>
+     * This function is called to have the robot move sideways
+     * in a left  direction
+     */
+    private void strafeLeft(double inches, double power) {
+        driveInches(inches, -power, power, power, -power);
+    }
+
+    /**
+     * Function: strafeRight
+     * <p>
+     * This function is called to have the robot move sideways
+     * in a right direction
+     */
+    private void strafeRight(double inches, double power) {
+        driveInches(inches, power, -power, -power, power);
+    }
+
+    /**
+     * Function: driveInches
+     * This function is called to have the robot move straight
+     * in a forward or reverse direction.
+     * <p>
+     * Strafe Forward = negative front wheels, positive back
+     * wheels
+     */
+    private void driveInches(double inches, double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) {
+        double counts = inches * AutoConstants.countsPerInch;
+
+        autoMethods.resetEncoders();
+        autoMethods.runUsingEncoders();
+
+        double voltage = motors.VoltSens.getVoltage();  // read current battery voltage
+
+        double leftFrontPowerCont = ((AutoConstants.batteryConst * leftFrontPower) / voltage);
+        double rightFrontPowerCont = ((AutoConstants.batteryConst * rightFrontPower) / voltage);
+        double leftBackPowerCont = ((AutoConstants.batteryConst * leftBackPower) / voltage);
+        double rightBackPowerCont = ((AutoConstants.batteryConst * rightBackPower) / voltage);
+
+        autoMethods.setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont);
+
+        while (opMode.opModeIsActive() &&
+            (Math.abs(motors.leftFront.getCurrentPosition()) +
+                Math.abs(motors.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) {
+            // Use gyro to drive in a straight line.
+            double correction = autoMethods.checkDirection();
+
+            // telemetry.addData("1 imu heading", lastAngles.firstAngle);
+            // telemetry.addData("2 global heading", globalAngle);
+            // telemetry.addData("3 correction", correction);
+            // telemetry.update();
+
+            autoMethods.setDrivePower(leftFrontPower - correction,
+                rightFrontPower + correction, leftBackPower - correction,
+                rightBackPower + correction);
+            opMode.idle();
+        }
+
+        autoMethods.setDrivePower(AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower);       // Stop all motors
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java

@@ -0,0 +1,63 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+public class AutoTurn {
+    private final MotorConstructor motors;
+    CommonAutoMethods autoMethods;
+    AutoConstants autoConst;
+    private final LinearOpMode opMode;
+
+    public AutoTurn(LinearOpMode opMode, HardwareMap hardwareMap) {
+        this.motors = new MotorConstructor(hardwareMap);
+        this.autoMethods = new CommonAutoMethods(opMode, hardwareMap);
+        this.autoConst = new AutoConstants();
+        this.opMode = opMode;
+    }
+
+    /**
+     * Rotate left or right the number of degrees. Does not support turning more than 180 degrees.
+     *
+     * @param degrees Degrees to turn, + is left - is right
+     */
+    public void rotate(int degrees, double power) {
+        double leftPower, rightPower;
+
+        // restart imu movement tracking.
+        resetAngle();
+
+        // getAngle() returns + when rotating counter clockwise (left) and - when rotating
+        // clockwise (right).
+        if (degrees < 0) {    // turn right.
+            leftPower = power;
+            rightPower = -power;
+        } else if (degrees > 0) {    // turn left.
+            leftPower = -power;
+            rightPower = power;
+        } else return;
+
+        // set power to rotate.
+        autoMethods.setDrivePower(leftPower, rightPower, leftPower, rightPower);
+
+        // turn the motors off.
+        autoMethods.setDrivePower(0.0, 0.0, 0.0, 0.0);
+
+        // wait for rotation to stop.
+        opMode.sleep(500);
+
+        // reset angle tracking on new heading.
+        resetAngle();
+    }
+
+
+    public void resetAngle() {
+        motors.imu.resetYaw();
+        YawPitchRollAngles lastAngles = motors.imu.getRobotYawPitchRollAngles();
+
+        int globalAngle = 0;
+    }
+}