From 2b6c09deda24e91ac703268810b9e615a34a5338 Mon Sep 17 00:00:00 2001
From: Jeffrey Mello <mrjeffreymello@gmail.com>
Date: Sat, 15 Nov 2025 21:52:27 -0500
Subject: [PATCH 1/2] Refactor controls into Utils_13233 package

Moved CommonControls and related classes to the Utils_13233 package for better modularity. Updated AutoMain, QuickAuto, and TeleOpMain to use new control classes. Removed old CommonControls and test files, and added new DriveControls, LaunchControls, RampControls, and MotorConstructor classes with corresponding tests. Updated Gradle workflow and run configurations to use new test targets.
---
 .github/workflows/gradle.yml                  |    2 +-
 ...l => 13233DECODE [All Unit Tests].run.xml} |    4 +-
 .run/CommonControlsUnitTest.run.xml           |    2 +-
 .../firstinspires/ftc/teamcode/AutoMain.java  | 1037 ++++++++---------
 .../ftc/teamcode/CommonControls.java          |  193 ---
 .../firstinspires/ftc/teamcode/QuickAuto.java |   44 +-
 .../ftc/teamcode/TeleOpMain.java              |   27 +-
 .../teamcode/Utils_13233/CommonControls.java  |   20 +
 .../teamcode/Utils_13233/DriveControls.java   |   83 ++
 .../teamcode/Utils_13233/LaunchControls.java  |   24 +
 .../Utils_13233/MotorConstructor.java         |   82 ++
 .../teamcode/Utils_13233/RampControls.java    |   34 +
 .../ftc/teamcode/CommonControlsTest.java      |  193 ---
 .../ftc/teamcode/DriveControlsTest.java       |   96 ++
 .../ftc/teamcode/LaunchControlsTest.java      |   47 +
 .../ftc/teamcode/MockMotorUtil.java           |   56 +
 .../ftc/teamcode/RampControlsTest.java        |   66 ++
 17 files changed, 1074 insertions(+), 936 deletions(-)
 rename .run/{13233DECODE [test].run.xml => 13233DECODE [All Unit Tests].run.xml} (80%)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/DriveControls.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java
 delete mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java
 create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
 create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java
 create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java
 create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java

diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
index de95b86..3bd520a 100644
--- a/.github/workflows/gradle.yml
+++ b/.github/workflows/gradle.yml
@@ -36,7 +36,7 @@ jobs:
         run: chmod +x gradlew
 
       - name: Run unit tests
-        run: ./gradlew :TeamCode:test --no-daemon --stacktrace
+        run: ./gradlew :TeamCode:testReleaseUnitTest --no-daemon --stacktrace
 
       - name: Build Release APK
         run: ./gradlew assembleRelease --no-daemon --stacktrace
diff --git a/.run/13233DECODE [test].run.xml b/.run/13233DECODE [All Unit Tests].run.xml
similarity index 80%
rename from .run/13233DECODE [test].run.xml
rename to .run/13233DECODE [All Unit Tests].run.xml
index ef4d996..70cc51c 100644
--- a/.run/13233DECODE [test].run.xml	
+++ b/.run/13233DECODE [All Unit Tests].run.xml	
@@ -1,5 +1,5 @@
 <component name="ProjectRunConfigurationManager">
-  <configuration default="false" name="13233DECODE [test]" type="GradleRunConfiguration" factoryName="Gradle">
+  <configuration default="false" name="13233DECODE [All Unit Tests]" type="GradleRunConfiguration" factoryName="Gradle" folderName="Unit Tests">
     <ExternalSystemSettings>
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
@@ -10,7 +10,7 @@
       </option>
       <option name="taskNames">
         <list>
-          <option value="test" />
+          <option value="testReleaseUnitTest" />
         </list>
       </option>
       <option name="vmOptions" />
diff --git a/.run/CommonControlsUnitTest.run.xml b/.run/CommonControlsUnitTest.run.xml
index edbc747..015a33c 100644
--- a/.run/CommonControlsUnitTest.run.xml
+++ b/.run/CommonControlsUnitTest.run.xml
@@ -1,5 +1,5 @@
 <component name="ProjectRunConfigurationManager">
-  <configuration default="false" name="CommonControlsUnitTest" type="GradleRunConfiguration" factoryName="Gradle">
+  <configuration default="false" name="CommonControlsUnitTest" type="GradleRunConfiguration" factoryName="Gradle" folderName="Unit Tests">
     <ExternalSystemSettings>
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
index 530baac..6f0dc7d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
@@ -1,519 +1,518 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.IMU;
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-
-// Telemetry - print on driver hub
-//
-
-
-// Robot Hardware Classes
-//
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.VoltageSensor;
-
-
-// import for IMU (gyroscope)
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-
-import kotlin.contracts.Returns;
-
-
-@Autonomous(name = "AutoMain", group = "Auto")
-
-public class AutoMain extends LinearOpMode {
-    CommonControls CommonControls = new CommonControls(hardwareMap);
-    private IMU imu;
-
-    private VoltageSensor VoltSens;
-
-
-    // Variables for imu
-    double globalAngle, correction;
-    YawPitchRollAngles lastAngles;
-
-    double voltage = 0;
-    double batteryConst = 13.5;
-
-
-    // Possible Autonomous Modes
-    public enum AutoMode {
-        AUTO_MODE_NOT_SELECTED,
-        AUTO_MODE_DEFAULT,
-    }
-
-
-    // Variables for driving with Encoders
-    private static final double wheelCircumference = 4.0 * Math.PI;
-    private static final double gearRatio = 18.9;                  // Rev HD Hex 20:1
-    private static final double countsPerRotation = 560.0;         // Rev HD Hex 20:1
-    private static final double scaleFactor = 9.0;                 // need to find scale factor!!!
-    private static final double countsPerInch = countsPerRotation / wheelCircumference
-        / gearRatio * scaleFactor;
-
-
-    // Global Variables
-    private final static double noPower = 0.0;
-    private final static double quarterPower = 0.25;
-    private final static double oneThirdPower = 0.34;
-    private final static double halfPower = 0.5;
-    private final static double threeQuartPower = 0.75;
-    private final static double fullPower = 1.0;
-
-
-    // Global Variables to store Game Specific Information
-    AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected
-
-
-    /**
-     * SelectAutoMode
-     * This function is use to select the autonomous code to be executed for this match
-     * Game pad 1 is used and the following buttons are used for selection:
-     * a - Red alliance Sample
-     * b - Blue alliance Sample
-     * x - Red alliance Specimen
-     * y - Blue alliance Specimen
-     *
-     * @return Selected mode
-     */
-    private AutoMode SelectAutoMode() {
-        // Local variable to store selected autonomous mode
-
-        AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED;
-        // Display autonomous mode not selected yet
-        telemetry.addData("AutoMode", "Not Selected");
-        telemetry.update();
-
-
-        // Loop until autonomous mode is selected
-        while (!isStopRequested() && autoMode == AutoMode.AUTO_MODE_NOT_SELECTED) {
-            autoMode = AutoMode.AUTO_MODE_DEFAULT;
-            idle();
-        }
-
-        // Display selected autonomous mode
-        telemetry.addData("Autonomous Mode", autoMode.toString());
-        telemetry.update();
-
-        // Wait for the user to release the button
-        while (!isStopRequested() && (gamepad1.a || gamepad1.b || gamepad1.x || gamepad1.y)) {
-            idle();
-        }
-
-        return autoMode;
-
-    } // end SelectAutoMode
-
-    /**
-     * Function: SelectDelayTime
-     * <p>
-     * This function is use to select how long to delay the start of the autonomous code.
-     * Game pad 1 is used and the following controls are used for selection:
-     * left bumper - decrease delay time by 1000 milliseconds (1 second)
-     * right bumper - increase delay time by 1000 milliseconds (1 second)
-     * a button -	set selected time
-     * note: if no delay time is needed, just select the a button. The default for the delay time
-     * is 0.
-     *
-     * @return Delay Time in milliseconds
-     */
-    private Integer SelectDelayTime() {
-        int delayTimeMilliseconds = 0;    // Initialize delay to be 0 seconds
-
-        // display delay time not set
-        telemetry.addData("Delay", "%d (Not Set)", delayTimeMilliseconds);
-        telemetry.addData("Left Bumper -", " decrease delay time by (1 second)");
-        telemetry.addData("Right bumper -", " increase delay time by (1 second)");
-        telemetry.addData("A", "No delay time is needed");
-        telemetry.update();
-
-
-        /* Select Delay time.
-           - Select 'a' button without hitting bumpers if no delay needed
-           - Use Left Bumper to decrease delay time
-           - Use Right bumper to increase delay time
-
-           Note: After entering delay time, use "a" button to set selected time
-        */
-
-        while (!isStopRequested() && !gamepad1.a) {
-            if (gamepad1.left_bumper) {
-                delayTimeMilliseconds -= 1000;
-
-                // ensure delay time does not go negative
-                if (delayTimeMilliseconds < 0) {
-                    delayTimeMilliseconds = 0;
-                }
-
-                // Wait for the bumper to be released
-                while (gamepad1.left_bumper) {
-                    idle();
-                }
-
-                telemetry.addData("Delay", "%d (decrease)", delayTimeMilliseconds);
-                telemetry.update();
-            }
-
-            if (gamepad1.right_bumper) {
-                delayTimeMilliseconds += 1000;
-
-                // ensure delay time is not greater than 10 seconds
-                if (delayTimeMilliseconds > 10000) {
-                    delayTimeMilliseconds = 10000;
-                }
-
-                while (gamepad1.right_bumper) {
-                    idle();
-                }
-                telemetry.addData("Delay", "%d (increase)", delayTimeMilliseconds);
-                telemetry.update();
-            }
-        }
-
-        // Wait for user to release the a button
-        while (!isStopRequested() && gamepad1.a) {
-            idle();
-        }
-
-
-        // Display selected delay time
-        telemetry.addData("Delay Time SET", delayTimeMilliseconds);
-        telemetry.update();
-        return delayTimeMilliseconds;       // returns selected delay time
-
-    } // end SelectDelayTime
-
-
-    // OpMode for autonomous code
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-        double tgtPower = 0;
-
-        VoltSens = hardwareMap.voltageSensor.get("Control Hub");
-
-
-
-        /* Setup IMU parameters */
-
-
-        // Retrieve and initialize the IMU. The IMU should be attached to
-        // IC2 port 0 on a Core Device Interface Module
-        imu = hardwareMap.get(IMU.class, "imu");
-
-        /* The next two lines define Hub orientation.
-         * The Default Orientation (shown) is when a hub is mounted horizontally with the printed
-         *  logo pointing UP and the USB port pointing FORWARD.
-         *
-         * To Do:  EDIT these two lines to match YOUR mounting configuration.
-         */
-        RevHubOrientationOnRobot.LogoFacingDirection logoDirection
-            = RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD;
-        RevHubOrientationOnRobot.UsbFacingDirection usbDirection
-            = RevHubOrientationOnRobot.UsbFacingDirection.DOWN;
-
-        telemetry.addData("Mode", "calibrating imu....");
-        telemetry.update();
-
-        try {
-            RevHubOrientationOnRobot orientationOnRobot =
-                new RevHubOrientationOnRobot(logoDirection, usbDirection);
-            imu.initialize(new IMU.Parameters(orientationOnRobot));
-
-            telemetry.addData("imu calib status", "calibrated");
-            telemetry.update();
-
-            // initialize imu global variables after calibrating imu
-            resetAngle();
-
-        } catch (IllegalArgumentException e) {
-            telemetry.addData("imu calib status", "failed - try again");
-            telemetry.update();
-        }
-
-
-        // set direction of motors
-        CommonControls.leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        CommonControls.rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
-        CommonControls.leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
-        CommonControls.rightBack.setDirection(DcMotorSimple.Direction.FORWARD);
-
-
-        // Create local variable to store selected autonomous mode
-        // and amount of delay time
-        int delayTimeMilliseconds = 0;
-
-        telemetry.setAutoClear(false);
-        telemetry.addData("Status", "Initializing");
-        telemetry.update();
-
-        /* Select Autonomous Mode, Parking Location and Delay time	  */
-
-        autoMode = SelectAutoMode();
-        // parkLocation = SelectParkLoc();
-        delayTimeMilliseconds = SelectDelayTime();
-
-
-        /* All required data entered.  Autonomous is initialized.	 */
-
-        telemetry.addData("Status", "Initialized");
-        telemetry.addData("mode", "waiting for start");
-        telemetry.addData(">", "Press Play to start op mode");
-        telemetry.update();
-
-        /* Wait for start of the match	 */
-        waitForStart();
-
-        telemetry.clearAll();               // clear display messages
-
-        telemetry.addData("Mode", "running");
-        telemetry.update();
-
-        resetEncoders();
-
-        // Delay start if needed
-        if (delayTimeMilliseconds > 0) {
-            // display delay status
-            telemetry.addData("Status", "Delaying");
-            telemetry.update();
-
-            // wait selected amount of time
-            sleep(delayTimeMilliseconds);
-
-            // display delay over and autonomous code is running
-            telemetry.addData("Status", "Running");
-            telemetry.update();
-
-        }
-
-        telemetry.clearAll();               // clear display messages
-
-        // Determine which autonomous code to run
-        switch (autoMode) {
-            case AUTO_MODE_DEFAULT:
-                defaultAuto();
-                break;
-            case AUTO_MODE_NOT_SELECTED:
-                // This one should not happen if it does do nothing
-                break;
-        }
-    }
-
-    private void defaultAuto() {
-        driveForward(10.0, quarterPower);
-    }
-
-
-    // Functions needed for driving auto
-    private void resetEncoders() {
-        CommonControls.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        CommonControls.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        CommonControls.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        CommonControls.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-    }
-
-    private void runUsingEncoders() {
-        CommonControls.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        CommonControls.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        CommonControls.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        CommonControls.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-    }
-
-    private void setDrivePower(double leftFrontPower, double rightFrontPower,
-                               double leftBackPower, double rightBackPower) {
-        CommonControls.rightFront.setPower(rightFrontPower);
-        CommonControls.rightBack.setPower(rightBackPower);
-        CommonControls.leftFront.setPower(leftFrontPower);
-        CommonControls.leftBack.setPower(leftBackPower);
-    }
-
-    /**
-     * 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: 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: 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.
-     */
-    private 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: 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 * countsPerInch;
-
-        resetEncoders();
-        runUsingEncoders();
-
-        voltage = VoltSens.getVoltage();  // read current battery voltage
-
-        double leftFrontPowerCont = ((batteryConst * leftFrontPower) / voltage);
-        double rightFrontPowerCont = ((batteryConst * rightFrontPower) / voltage);
-        double leftBackPowerCont = ((batteryConst * leftBackPower) / voltage);
-        double rightBackPowerCont = ((batteryConst * rightBackPower) / voltage);
-
-        setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont);
-
-        while (opModeIsActive() &&
-            (Math.abs(CommonControls.leftFront.getCurrentPosition()) +
-                Math.abs(CommonControls.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) {
-            // Use gyro to drive in a straight line.
-            correction = checkDirection();
-
-            // telemetry.addData("1 imu heading", lastAngles.firstAngle);
-            // telemetry.addData("2 global heading", globalAngle);
-            // telemetry.addData("3 correction", correction);
-            // telemetry.update();
-
-            setDrivePower(leftFrontPower - correction,
-                rightFrontPower + correction, leftBackPower - correction,
-                rightBackPower + correction);
-            idle();
-        }
-
-        setDrivePower(noPower, noPower, noPower, noPower);       // Stop all motors
-    }
-
-    // Functions for turning and checking robot angle for correction
-
-    /* Resets the cumulative angle tracking to zero.   */
-
-    private void resetAngle() {
-        imu.resetYaw();
-        lastAngles = imu.getRobotYawPitchRollAngles();
-
-        globalAngle = 0;
-    }
-
-    /**
-     * Get current cumulative angle rotation from last reset.
-     *
-     * @return Angle in degrees. + = left, - = right.
-     */
-    private double getAngle() {
-        // We experimentally determined the Z axis is the axis we want to use for heading angle.
-        // We have to process the angle because the imu works in euler angles so the Z axis is
-        // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes
-        // 180 degrees. We detect this transition and track the total cumulative angle of rotation.
-
-        // Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
-        YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
-
-        double deltaAngle = orientation.getYaw(AngleUnit.DEGREES) - lastAngles.getYaw(AngleUnit.DEGREES);
-
-        if (deltaAngle < -180)
-            deltaAngle += 360;
-        else if (deltaAngle > 180)
-            deltaAngle -= 360;
-
-        globalAngle += deltaAngle;
-
-        lastAngles = orientation;
-
-        return globalAngle;
-    }
-
-    /**
-     * See if we are moving in a straight line and if not return a power correction value.
-     *
-     * @return Power adjustment, + is adjust left - is adjust right.
-     */
-    private double checkDirection() {
-        // The gain value determines how sensitive the correction is to direction changes.
-        // You will have to experiment with your robot to get small smooth direction changes
-        // to stay on a straight line.
-        double correction, angle, gain = .05;
-
-        angle = getAngle();
-
-        if (angle == 0)
-            correction = 0;             // no adjustment.
-        else
-            correction = -angle;        // reverse sign of angle for correction.
-
-        correction = correction * gain;
-
-        return correction;
-    }
-
-    /**
-     * 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
-     */
-    private 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.
-        setDrivePower(leftPower, rightPower, leftPower, rightPower);
-
-        // turn the motors off.
-        setDrivePower(0.0, 0.0, 0.0, 0.0);
-
-        // wait for rotation to stop.
-        sleep(500);
-
-        // reset angle tracking on new heading.
-        resetAngle();
-    }
-
-}
+//package org.firstinspires.ftc.teamcode;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//import com.qualcomm.robotcore.hardware.IMU;
+//import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+//
+//
+//// Telemetry - print on driver hub
+////
+//
+//
+//// Robot Hardware Classes
+////
+//import com.qualcomm.robotcore.hardware.DcMotor;
+//import com.qualcomm.robotcore.hardware.DcMotorSimple;
+//import com.qualcomm.robotcore.hardware.VoltageSensor;
+//
+//
+//// import for IMU (gyroscope)
+//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+//import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+//import org.firstinspires.ftc.teamcode.Utils_13233.CommonControls;
+//
+//
+//@Autonomous(name = "AutoMain", group = "Auto")
+//
+//public class AutoMain extends LinearOpMode {
+//    CommonControls CommonControls = new CommonControls(hardwareMap);
+//    private IMU imu;
+//
+//    private VoltageSensor VoltSens;
+//
+//
+//    // Variables for imu
+//    double globalAngle, correction;
+//    YawPitchRollAngles lastAngles;
+//
+//    double voltage = 0;
+//    double batteryConst = 13.5;
+//
+//
+//    // Possible Autonomous Modes
+//    public enum AutoMode {
+//        AUTO_MODE_NOT_SELECTED,
+//        AUTO_MODE_DEFAULT,
+//    }
+//
+//
+//    // Variables for driving with Encoders
+//    private static final double wheelCircumference = 4.0 * Math.PI;
+//    private static final double gearRatio = 18.9;                  // Rev HD Hex 20:1
+//    private static final double countsPerRotation = 560.0;         // Rev HD Hex 20:1
+//    private static final double scaleFactor = 9.0;                 // need to find scale factor!!!
+//    private static final double countsPerInch = countsPerRotation / wheelCircumference
+//        / gearRatio * scaleFactor;
+//
+//
+//    // Global Variables
+//    private final static double noPower = 0.0;
+//    private final static double quarterPower = 0.25;
+//    private final static double oneThirdPower = 0.34;
+//    private final static double halfPower = 0.5;
+//    private final static double threeQuartPower = 0.75;
+//    private final static double fullPower = 1.0;
+//
+//
+//    // Global Variables to store Game Specific Information
+//    AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected
+//
+//
+//    /**
+//     * SelectAutoMode
+//     * This function is use to select the autonomous code to be executed for this match
+//     * Game pad 1 is used and the following buttons are used for selection:
+//     * a - Red alliance Sample
+//     * b - Blue alliance Sample
+//     * x - Red alliance Specimen
+//     * y - Blue alliance Specimen
+//     *
+//     * @return Selected mode
+//     */
+//    private AutoMode SelectAutoMode() {
+//        // Local variable to store selected autonomous mode
+//
+//        AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED;
+//        // Display autonomous mode not selected yet
+//        telemetry.addData("AutoMode", "Not Selected");
+//        telemetry.update();
+//
+//
+//        // Loop until autonomous mode is selected
+//        while (!isStopRequested() && autoMode == AutoMode.AUTO_MODE_NOT_SELECTED) {
+//            autoMode = AutoMode.AUTO_MODE_DEFAULT;
+//            idle();
+//        }
+//
+//        // Display selected autonomous mode
+//        telemetry.addData("Autonomous Mode", autoMode.toString());
+//        telemetry.update();
+//
+//        // Wait for the user to release the button
+//        while (!isStopRequested() && (gamepad1.a || gamepad1.b || gamepad1.x || gamepad1.y)) {
+//            idle();
+//        }
+//
+//        return autoMode;
+//
+//    } // end SelectAutoMode
+//
+//    /**
+//     * Function: SelectDelayTime
+//     * <p>
+//     * This function is use to select how long to delay the start of the autonomous code.
+//     * Game pad 1 is used and the following controls are used for selection:
+//     * left bumper - decrease delay time by 1000 milliseconds (1 second)
+//     * right bumper - increase delay time by 1000 milliseconds (1 second)
+//     * a button -	set selected time
+//     * note: if no delay time is needed, just select the a button. The default for the delay time
+//     * is 0.
+//     *
+//     * @return Delay Time in milliseconds
+//     */
+//    private Integer SelectDelayTime() {
+//        int delayTimeMilliseconds = 0;    // Initialize delay to be 0 seconds
+//
+//        // display delay time not set
+//        telemetry.addData("Delay", "%d (Not Set)", delayTimeMilliseconds);
+//        telemetry.addData("Left Bumper -", " decrease delay time by (1 second)");
+//        telemetry.addData("Right bumper -", " increase delay time by (1 second)");
+//        telemetry.addData("A", "No delay time is needed");
+//        telemetry.update();
+//
+//
+//        /* Select Delay time.
+//           - Select 'a' button without hitting bumpers if no delay needed
+//           - Use Left Bumper to decrease delay time
+//           - Use Right bumper to increase delay time
+//
+//           Note: After entering delay time, use "a" button to set selected time
+//        */
+//
+//        while (!isStopRequested() && !gamepad1.a) {
+//            if (gamepad1.left_bumper) {
+//                delayTimeMilliseconds -= 1000;
+//
+//                // ensure delay time does not go negative
+//                if (delayTimeMilliseconds < 0) {
+//                    delayTimeMilliseconds = 0;
+//                }
+//
+//                // Wait for the bumper to be released
+//                while (gamepad1.left_bumper) {
+//                    idle();
+//                }
+//
+//                telemetry.addData("Delay", "%d (decrease)", delayTimeMilliseconds);
+//                telemetry.update();
+//            }
+//
+//            if (gamepad1.right_bumper) {
+//                delayTimeMilliseconds += 1000;
+//
+//                // ensure delay time is not greater than 10 seconds
+//                if (delayTimeMilliseconds > 10000) {
+//                    delayTimeMilliseconds = 10000;
+//                }
+//
+//                while (gamepad1.right_bumper) {
+//                    idle();
+//                }
+//                telemetry.addData("Delay", "%d (increase)", delayTimeMilliseconds);
+//                telemetry.update();
+//            }
+//        }
+//
+//        // Wait for user to release the a button
+//        while (!isStopRequested() && gamepad1.a) {
+//            idle();
+//        }
+//
+//
+//        // Display selected delay time
+//        telemetry.addData("Delay Time SET", delayTimeMilliseconds);
+//        telemetry.update();
+//        return delayTimeMilliseconds;       // returns selected delay time
+//
+//    } // end SelectDelayTime
+//
+//
+//    // OpMode for autonomous code
+//    @Override
+//    public void runOpMode() throws InterruptedException {
+//
+//        double tgtPower = 0;
+//
+//        VoltSens = hardwareMap.voltageSensor.get("Control Hub");
+//
+//
+//
+//        /* Setup IMU parameters */
+//
+//
+//        // Retrieve and initialize the IMU. The IMU should be attached to
+//        // IC2 port 0 on a Core Device Interface Module
+//        imu = hardwareMap.get(IMU.class, "imu");
+//
+//        /* The next two lines define Hub orientation.
+//         * The Default Orientation (shown) is when a hub is mounted horizontally with the printed
+//         *  logo pointing UP and the USB port pointing FORWARD.
+//         *
+//         * To Do:  EDIT these two lines to match YOUR mounting configuration.
+//         */
+//        RevHubOrientationOnRobot.LogoFacingDirection logoDirection
+//            = RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD;
+//        RevHubOrientationOnRobot.UsbFacingDirection usbDirection
+//            = RevHubOrientationOnRobot.UsbFacingDirection.DOWN;
+//
+//        telemetry.addData("Mode", "calibrating imu....");
+//        telemetry.update();
+//
+//        try {
+//            RevHubOrientationOnRobot orientationOnRobot =
+//                new RevHubOrientationOnRobot(logoDirection, usbDirection);
+//            imu.initialize(new IMU.Parameters(orientationOnRobot));
+//
+//            telemetry.addData("imu calib status", "calibrated");
+//            telemetry.update();
+//
+//            // initialize imu global variables after calibrating imu
+//            resetAngle();
+//
+//        } catch (IllegalArgumentException e) {
+//            telemetry.addData("imu calib status", "failed - try again");
+//            telemetry.update();
+//        }
+//
+//
+//        // set direction of motors
+//        CommonControls.leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+//        CommonControls.rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
+//        CommonControls.leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
+//        CommonControls.rightBack.setDirection(DcMotorSimple.Direction.FORWARD);
+//
+//
+//        // Create local variable to store selected autonomous mode
+//        // and amount of delay time
+//        int delayTimeMilliseconds = 0;
+//
+//        telemetry.setAutoClear(false);
+//        telemetry.addData("Status", "Initializing");
+//        telemetry.update();
+//
+//        /* Select Autonomous Mode, Parking Location and Delay time	  */
+//
+//        autoMode = SelectAutoMode();
+//        // parkLocation = SelectParkLoc();
+//        delayTimeMilliseconds = SelectDelayTime();
+//
+//
+//        /* All required data entered.  Autonomous is initialized.	 */
+//
+//        telemetry.addData("Status", "Initialized");
+//        telemetry.addData("mode", "waiting for start");
+//        telemetry.addData(">", "Press Play to start op mode");
+//        telemetry.update();
+//
+//        /* Wait for start of the match	 */
+//        waitForStart();
+//
+//        telemetry.clearAll();               // clear display messages
+//
+//        telemetry.addData("Mode", "running");
+//        telemetry.update();
+//
+//        resetEncoders();
+//
+//        // Delay start if needed
+//        if (delayTimeMilliseconds > 0) {
+//            // display delay status
+//            telemetry.addData("Status", "Delaying");
+//            telemetry.update();
+//
+//            // wait selected amount of time
+//            sleep(delayTimeMilliseconds);
+//
+//            // display delay over and autonomous code is running
+//            telemetry.addData("Status", "Running");
+//            telemetry.update();
+//
+//        }
+//
+//        telemetry.clearAll();               // clear display messages
+//
+//        // Determine which autonomous code to run
+//        switch (autoMode) {
+//            case AUTO_MODE_DEFAULT:
+//                defaultAuto();
+//                break;
+//            case AUTO_MODE_NOT_SELECTED:
+//                // This one should not happen if it does do nothing
+//                break;
+//        }
+//    }
+//
+//    private void defaultAuto() {
+//        driveForward(10.0, quarterPower);
+//    }
+//
+//
+//    // Functions needed for driving auto
+//    private void resetEncoders() {
+//        CommonControls.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//        CommonControls.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//        CommonControls.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//        CommonControls.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//    }
+//
+//    private void runUsingEncoders() {
+//        CommonControls.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+//        CommonControls.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+//        CommonControls.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+//        CommonControls.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+//    }
+//
+//    private void setDrivePower(double leftFrontPower, double rightFrontPower,
+//                               double leftBackPower, double rightBackPower) {
+//        CommonControls.rightFront.setPower(rightFrontPower);
+//        CommonControls.rightBack.setPower(rightBackPower);
+//        CommonControls.leftFront.setPower(leftFrontPower);
+//        CommonControls.leftBack.setPower(leftBackPower);
+//    }
+//
+//    /**
+//     * 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: 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: 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.
+//     */
+//    private 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: 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 * countsPerInch;
+//
+//        resetEncoders();
+//        runUsingEncoders();
+//
+//        voltage = VoltSens.getVoltage();  // read current battery voltage
+//
+//        double leftFrontPowerCont = ((batteryConst * leftFrontPower) / voltage);
+//        double rightFrontPowerCont = ((batteryConst * rightFrontPower) / voltage);
+//        double leftBackPowerCont = ((batteryConst * leftBackPower) / voltage);
+//        double rightBackPowerCont = ((batteryConst * rightBackPower) / voltage);
+//
+//        setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont);
+//
+//        while (opModeIsActive() &&
+//            (Math.abs(CommonControls.leftFront.getCurrentPosition()) +
+//                Math.abs(CommonControls.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) {
+//            // Use gyro to drive in a straight line.
+//            correction = checkDirection();
+//
+//            // telemetry.addData("1 imu heading", lastAngles.firstAngle);
+//            // telemetry.addData("2 global heading", globalAngle);
+//            // telemetry.addData("3 correction", correction);
+//            // telemetry.update();
+//
+//            setDrivePower(leftFrontPower - correction,
+//                rightFrontPower + correction, leftBackPower - correction,
+//                rightBackPower + correction);
+//            idle();
+//        }
+//
+//        setDrivePower(noPower, noPower, noPower, noPower);       // Stop all motors
+//    }
+//
+//    // Functions for turning and checking robot angle for correction
+//
+//    /* Resets the cumulative angle tracking to zero.   */
+//
+//    private void resetAngle() {
+//        imu.resetYaw();
+//        lastAngles = imu.getRobotYawPitchRollAngles();
+//
+//        globalAngle = 0;
+//    }
+//
+//    /**
+//     * Get current cumulative angle rotation from last reset.
+//     *
+//     * @return Angle in degrees. + = left, - = right.
+//     */
+//    private double getAngle() {
+//        // We experimentally determined the Z axis is the axis we want to use for heading angle.
+//        // We have to process the angle because the imu works in euler angles so the Z axis is
+//        // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes
+//        // 180 degrees. We detect this transition and track the total cumulative angle of rotation.
+//
+//        // Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+//        YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+//
+//        double deltaAngle = orientation.getYaw(AngleUnit.DEGREES) - lastAngles.getYaw(AngleUnit.DEGREES);
+//
+//        if (deltaAngle < -180)
+//            deltaAngle += 360;
+//        else if (deltaAngle > 180)
+//            deltaAngle -= 360;
+//
+//        globalAngle += deltaAngle;
+//
+//        lastAngles = orientation;
+//
+//        return globalAngle;
+//    }
+//
+//    /**
+//     * See if we are moving in a straight line and if not return a power correction value.
+//     *
+//     * @return Power adjustment, + is adjust left - is adjust right.
+//     */
+//    private double checkDirection() {
+//        // The gain value determines how sensitive the correction is to direction changes.
+//        // You will have to experiment with your robot to get small smooth direction changes
+//        // to stay on a straight line.
+//        double correction, angle, gain = .05;
+//
+//        angle = getAngle();
+//
+//        if (angle == 0)
+//            correction = 0;             // no adjustment.
+//        else
+//            correction = -angle;        // reverse sign of angle for correction.
+//
+//        correction = correction * gain;
+//
+//        return correction;
+//    }
+//
+//    /**
+//     * 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
+//     */
+//    private 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.
+//        setDrivePower(leftPower, rightPower, leftPower, rightPower);
+//
+//        // turn the motors off.
+//        setDrivePower(0.0, 0.0, 0.0, 0.0);
+//
+//        // wait for rotation to stop.
+//        sleep(500);
+//
+//        // reset angle tracking on new heading.
+//        resetAngle();
+//    }
+//
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java
deleted file mode 100644
index 368797c..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java
+++ /dev/null
@@ -1,193 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-
-/**
- * Class the holds common functions and variable for the 13233 DECODE robot
- */
-public class CommonControls {
-    /**
-     * Front left drive motor
-     */
-    public DcMotor leftFront;
-    /**
-     * Front right drive motor
-     */
-    public DcMotor rightFront;
-    /**
-     * Back left drive motor
-     */
-    public DcMotor leftBack;
-    /**
-     * Right back drive motor
-     */
-    public DcMotor rightBack;
-
-
-    /**
-     * The motor for that controls the ball intake
-     */
-    private DcMotor intake;
-
-    /**
-     * One of the motors used to shoot the balls
-     */
-    private DcMotor Launcher;
-    /**
-     * One of the motors used to shoot the balls
-     */
-    private DcMotor Launcher2;
-
-
-    /**
-     * One of the servos used to carry the balls up the ramp
-     */
-    private CRServo rampServo1;
-    /**
-     * One of the servos used to carry the balls up the ramp
-     */
-    private CRServo rampServo2;
-    private CRServo rampServo3;
-    private CRServo rampServo4;
-
-    /**
-     * Constructor for the CommonControls class
-     *
-     * @param hardwareMap The hardware map for the robot
-     */
-    public CommonControls(HardwareMap hardwareMap) {
-        // Map main Drive Motors
-        leftFront = hardwareMap.get(DcMotor.class, "leftFront");
-        rightFront = hardwareMap.get(DcMotor.class, "rightFront");
-        leftBack = hardwareMap.get(DcMotor.class, "leftBack");
-        rightBack = hardwareMap.get(DcMotor.class, "rightBack");
-
-        // Map Intake Motor
-        intake = hardwareMap.get(DcMotor.class, "intake");
-
-        // Map the launcher Prototype
-        Launcher = hardwareMap.dcMotor.get("Launcher");
-        Launcher2 = hardwareMap.dcMotor.get("Launcher2");
-        rampServo1 = hardwareMap.crservo.get("rampServo1");
-        rampServo2 = hardwareMap.crservo.get("rampServo2");
-        rampServo3 = hardwareMap.crservo.get("rampServo3");
-        rampServo4 = hardwareMap.crservo.get("rampServo4");
-        // Set direction of the main drive motors
-        leftFront.setDirection(DcMotorSimple.Direction.FORWARD);
-        rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftBack.setDirection(DcMotorSimple.Direction.FORWARD);
-        rightBack.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        // Set the direction of the intake motor
-        intake.setDirection(DcMotorSimple.Direction.REVERSE);
-    }
-
-
-    // TODO:
-    //        Implement field oriented drive
-
-    /**
-     * Updates all the drive controls based on the current gamepad stick positions
-     *
-     * @param controlLeftStickY Left stick Y setting, up/down
-     * @param controlLeftStickX Left stick X setting, left/right
-     * @param controlRightStick Right Stick setting, controls left right turn
-     * @param speedLimiter      Trigger setting, controls speed of the robot
-     */
-    public void setDrivePower(float controlLeftStickY, float controlLeftStickX,
-                              float controlRightStick, float speedLimiter) {
-        // For readability and flexibility for future control input changes
-        //noinspection UnnecessaryLocalVariable
-        float forward = controlLeftStickY;
-        //noinspection UnnecessaryLocalVariable
-        float strafe = controlLeftStickX;
-        //noinspection UnnecessaryLocalVariable
-        float rotate = controlRightStick;
-        float speed = Math.max(1.0f - speedLimiter, 0.5f);
-
-
-        // Calculate the power for each motor
-        float frontRightPower = (forward + strafe + rotate) * (speed);
-        float frontLeftPower = (forward - strafe - rotate) * (speed);
-        float backRightPower = (forward - strafe + rotate) * (speed);
-        float backLeftPower = (forward + strafe - rotate) * (speed);
-
-        // Set the power for each of the motors
-        rightFront.setPower(frontRightPower);
-        leftFront.setPower(frontLeftPower);
-        rightBack.setPower(backRightPower);
-        leftBack.setPower(backLeftPower);
-    }
-
-    /**
-     * Updates just the forward and back movement based on controller stick position
-     *
-     * @param controlLeftStickY Left stick Y setting, up/down
-     */
-    public void setDrivePower(float controlLeftStickY) {
-        setDrivePower(controlLeftStickY, 0.0f, 0.0f, 0.0f);
-    }
-
-
-    /**
-     * Sets the power of all of the launch motors depending on the value of launchInput
-     *
-     * @param launchInput Button mapped to launch input
-     */
-    void setLaunchPower(boolean launchInput) {
-        double power = launchInput ? 1.0 : 0.0;
-
-        // Set the the power value to the motors
-        Launcher.setPower(-power);
-        Launcher2.setPower(power);
-    }
-
-    /**
-     * Controls the spin direction of the intake wheel based on controller buttons
-     *
-     * @param intakeForwardInput Button mapped to forward input
-     * @param intakeReverseInput Button mapped to reverse input
-     */
-    void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverseInput) {
-        double intakePower = 0;
-        // ^ is the XOR operator, will return true if only one variable is true
-        // If intakeForwardInput OR intakeReverseInput is true then this is true, but not
-        // if both are true, which is why this differs from the || logical or operator
-        if (intakeForwardInput ^ intakeReverseInput) {
-            // This uses an inline-if statement which is useful when assigning values to variables
-            // This says set power = 1 if intakeForwardInput is true, else set it to -1
-            intakePower = intakeForwardInput ? 1 : -1;
-        }
-        rampServo1.setPower(-intakePower);
-        rampServo2.setPower(intakePower);
-        rampServo3.setPower(intakePower);
-        rampServo4.setPower(-intakePower);
-        intake.setPower(intakePower);
-    }
-
-    void setDriveBrake() {
-        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-    }
-
-    void setDriveFloat() {
-        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-    }
-
-    void setDriveMotorZeroPowerBehavior(boolean input) {
-        if (input) {
-            setDriveBrake();
-        } else {
-            setDriveFloat();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java
index 5492554..73afa16 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java
@@ -1,22 +1,22 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.*;
-
-@Autonomous(name = "QuickAuto")
-public class QuickAuto extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        telemetry.addData("Status", "Initialized");
-        telemetry.update();
-        // Create a new CommonControls object
-        CommonControls CommonControls = new CommonControls(hardwareMap);
-
-        //CommonControls.setDriveBrake();
-        waitForStart();
-        if (opModeIsActive()) {// OpMode loop
-            CommonControls.setDrivePower(-1.0f);
-            sleep(500);
-            CommonControls.setDrivePower(0);
-        }
-    }
-}
+//package org.firstinspires.ftc.teamcode;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.*;
+//
+//@Autonomous(name = "QuickAuto")
+//public class QuickAuto extends LinearOpMode {
+//    @Override
+//    public void runOpMode() {
+//        telemetry.addData("Status", "Initialized");
+//        telemetry.update();
+//        // Create a new CommonControls object
+//        CommonControls CommonControls = new CommonControls(hardwareMap);
+//
+//        //CommonControls.setDriveBrake();
+//        waitForStart();
+//        if (opModeIsActive()) {// OpMode loop
+//            CommonControls.setDrivePower(-1.0f);
+//            sleep(500);
+//            CommonControls.setDrivePower(0);
+//        }
+//    }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
index bc7dfa9..c95d742 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
@@ -3,16 +3,33 @@
 
 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;
 
 
 @TeleOp(name = "TeleOpMain", group = "Drive")
 public class TeleOpMain extends LinearOpMode {
 
+    // Constructors for the utils classes
+    private final DriveControls drive;
+    private final LaunchControls launch;
+    private final RampControls ramp;
+
+    public TeleOpMain(HardwareMap hardwareMap) {
+        this.drive = new DriveControls(hardwareMap);
+        this.launch = new LaunchControls(hardwareMap);
+        this.ramp = new RampControls(hardwareMap);
+    }
+
+
     //This function is executed when this Op Mode is selected from the Driver Station
     @Override
     public void runOpMode() {
         // Initialize the CommonControls class
-        CommonControls CommonControls = new CommonControls(hardwareMap);
 
         // The waitForStart() function will wait for the start button to begin
         // DON'T WRITE ANY CODE AFTER THE WAIT FOR START UNTIL THE "while (opModIsActive())"
@@ -25,21 +42,21 @@ public void runOpMode() {
             telemetry.update();
 
             // Set the power to the launch motors based while the x button is being pressed
-            CommonControls.setLaunchPower(gamepad1.x);
+            launch.setLaunchPower(gamepad1.x);
 
             //Add option to enable brakes when sharbell holds a
-            CommonControls.setDriveMotorZeroPowerBehavior(gamepad1.a);
+            drive.setDriveMotorZeroPowerBehavior(gamepad1.a);
 
             // Set all of the intake motors and servos to go forward when dpad up is pressed
             // and reverse when dpad down is pressed
-            CommonControls.setIntakeDirection(gamepad2.dpad_up, gamepad2.dpad_down);
+            ramp.setIntakeDirection(gamepad2.dpad_up, gamepad2.dpad_down);
             // Wait for motors to speed up before changing value
             // While it is bad practice to put a sleep in a loop it is the only way to (that I know)
             // to make the intake not jitter I also
 
 
             // Sets the power to the drive motors based on current gamepad inputs
-            CommonControls.setDrivePower(gamepad1.left_stick_y, gamepad1.left_stick_x,
+            drive.setDrivePower(gamepad1.left_stick_y, gamepad1.left_stick_x,
                 gamepad1.right_stick_x, gamepad1.left_trigger);
         }
         sleep(50);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java
new file mode 100644
index 0000000..393b1fd
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+
+/**
+ * Class the holds common functions and variable for the 13233 DECODE robot
+ */
+public class CommonControls{
+    private final MotorConstructor motors;
+
+    public CommonControls(HardwareMap hardwareMap) {
+        this.motors = new MotorConstructor(hardwareMap);
+    }
+
+
+
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/DriveControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/DriveControls.java
new file mode 100644
index 0000000..7412ad4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/DriveControls.java
@@ -0,0 +1,83 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class DriveControls {
+    private final MotorConstructor motors;
+
+    public DriveControls(HardwareMap hardwareMap) {
+        this.motors = new MotorConstructor(hardwareMap);
+    }
+
+    // TODO:
+    //        Implement field oriented drive
+
+    /**
+     * Updates all the drive controls based on the current gamepad stick positions
+     *
+     * @param controlLeftStickY Left stick Y setting, up/down
+     * @param controlLeftStickX Left stick X setting, left/right
+     * @param controlRightStick Right Stick setting, controls left right turn
+     * @param speedLimiter      Trigger setting, controls speed of the robot
+     */
+    public void setDrivePower(float controlLeftStickY, float controlLeftStickX,
+                              float controlRightStick, float speedLimiter) {
+        // For readability and flexibility for future control input changes
+        //noinspection UnnecessaryLocalVariable
+        float forward = controlLeftStickY;
+        //noinspection UnnecessaryLocalVariable
+        float strafe = controlLeftStickX;
+        //noinspection UnnecessaryLocalVariable
+        float rotate = controlRightStick;
+        float speed = Math.max(1.0f - speedLimiter, 0.5f);
+
+
+        // Calculate the power for each motor
+        float frontRightPower = (forward + strafe + rotate) * (speed);
+        float frontLeftPower = (forward - strafe - rotate) * (speed);
+        float backRightPower = (forward - strafe + rotate) * (speed);
+        float backLeftPower = (forward + strafe - rotate) * (speed);
+
+        // Set the power for each of the motors
+        motors.rightFront.setPower(frontRightPower);
+        motors.leftFront.setPower(frontLeftPower);
+        motors.rightBack.setPower(backRightPower);
+        motors.leftBack.setPower(backLeftPower);
+    }
+
+    /**
+     * Updates just the forward and back movement based on controller stick position
+     *
+     * @param controlLeftStickY Left stick Y setting, up/down
+     */
+    public void setDrivePower(float controlLeftStickY) {
+        setDrivePower(controlLeftStickY, 0.0f, 0.0f, 0.0f);
+    }
+
+    // TODO: Create JavaDoc
+    void setDriveBrake() {
+        motors.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        motors.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        motors.leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        motors.rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    // TODO: Create JavaDoc
+    void setDriveFloat() {
+        motors.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        motors.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        motors.leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        motors.rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+    }
+
+    // TODO: Create JavaDoc
+    public void setDriveMotorZeroPowerBehavior(boolean input) {
+        if (input) {
+            setDriveBrake();
+        } else {
+            setDriveFloat();
+        }
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java
new file mode 100644
index 0000000..36f4da2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class LaunchControls {
+    private final MotorConstructor motors;
+
+    public LaunchControls(HardwareMap hardwareMap) {
+        this.motors = new MotorConstructor(hardwareMap);
+    }
+
+    /**
+     * Sets the power of all of the launch motors depending on the value of launchInput
+     *
+     * @param launchInput Button mapped to launch input
+     */
+    public void setLaunchPower(boolean launchInput) {
+        double power = launchInput ? 1.0 : 0.0;
+
+        // Set the the power value to the motors
+        motors.Launcher.setPower(-power);
+        motors.Launcher2.setPower(power);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java
new file mode 100644
index 0000000..4895072
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java
@@ -0,0 +1,82 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class MotorConstructor {
+    /**
+     * Front left drive motor
+     */
+    public DcMotor leftFront;
+    /**
+     * Front right drive motor
+     */
+    public DcMotor rightFront;
+    /**
+     * Back left drive motor
+     */
+    public DcMotor leftBack;
+    /**
+     * Right back drive motor
+     */
+    public DcMotor rightBack;
+
+
+    /**
+     * The motor for that controls the ball intake
+     */
+    public DcMotor intake;
+
+    /**
+     * One of the motors used to shoot the balls
+     */
+    public DcMotor Launcher;
+    /**
+     * One of the motors used to shoot the balls
+     */
+    public DcMotor Launcher2;
+
+
+    /**
+     * One of the servos used to carry the balls up the ramp
+     */
+    public CRServo rampServo1;
+    /**
+     * One of the servos used to carry the balls up the ramp
+     */
+    public CRServo rampServo2;
+    public CRServo rampServo3;
+    public CRServo rampServo4;
+
+    public MotorConstructor(HardwareMap hardwareMap){
+        // Map main Drive Motors
+        leftFront = hardwareMap.get(DcMotor.class, "leftFront");
+        rightFront = hardwareMap.get(DcMotor.class, "rightFront");
+        leftBack = hardwareMap.get(DcMotor.class, "leftBack");
+        rightBack = hardwareMap.get(DcMotor.class, "rightBack");
+
+        // Map Intake Motor
+        intake = hardwareMap.get(DcMotor.class, "intake");
+
+        // Map the launcher Prototype
+        Launcher  = hardwareMap.get(DcMotor.class, "Launcher");
+        Launcher2 = hardwareMap.get(DcMotor.class, "Launcher2");
+
+        rampServo1 = hardwareMap.get(CRServo.class, "rampServo1");
+        rampServo2 = hardwareMap.get(CRServo.class, "rampServo2");
+        rampServo3 = hardwareMap.get(CRServo.class, "rampServo3");
+        rampServo4 = hardwareMap.get(CRServo.class, "rampServo4");
+
+        // Set direction of the main drive motors
+        leftFront.setDirection(DcMotorSimple.Direction.FORWARD);
+        rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftBack.setDirection(DcMotorSimple.Direction.FORWARD);
+        rightBack.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // Set the direction of the intake motor
+        intake.setDirection(DcMotorSimple.Direction.REVERSE);
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java
new file mode 100644
index 0000000..2de0485
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java
@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.Utils_13233;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class RampControls {
+    /**
+     * Controls the spin direction of the intake wheel based on controller buttons
+     *
+     * @param intakeForwardInput Button mapped to forward input
+     * @param intakeReverseInput Button mapped to reverse input
+     */
+    private final MotorConstructor motors;
+
+    public RampControls(HardwareMap hardwareMap) {
+        this.motors = new MotorConstructor(hardwareMap);
+    }
+    public void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverseInput) {
+        double intakePower = 0;
+        // ^ is the XOR operator, will return true if only one variable is true
+        // If intakeForwardInput OR intakeReverseInput is true then this is true, but not
+        // if both are true, which is why this differs from the || logical or operator
+        if (intakeForwardInput ^ intakeReverseInput) {
+            // This uses an inline-if statement which is useful when assigning values to variables
+            // This says set power = 1 if intakeForwardInput is true, else set it to -1
+            intakePower = intakeForwardInput ? 1 : -1;
+        }
+        motors.rampServo1.setPower(-intakePower);
+        motors.rampServo2.setPower(intakePower);
+        motors.rampServo3.setPower(intakePower);
+        motors.rampServo4.setPower(-intakePower);
+        motors.intake.setPower(intakePower);
+    }
+
+}
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java
deleted file mode 100644
index c32d328..0000000
--- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java
+++ /dev/null
@@ -1,193 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.mockito.Mockito.mock;
-import static org.mockito.Mockito.verify;
-import static org.mockito.Mockito.when;
-
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.DisplayName;
-import org.junit.jupiter.api.Test;
-
-public class CommonControlsTest {
-
-    private DcMotor leftFront, rightFront, leftBack, rightBack, intake, Launcher, Launcher2;
-    private CRServo rampServo1, rampServo2, rampServo3, rampServo4;
-    private HardwareMap hardwareMap;
-    private CommonControls controls;
-
-    @BeforeEach
-    void setUp() {
-        // Mock all motors
-        leftFront = mock(DcMotor.class);
-        rightFront = mock(DcMotor.class);
-        leftBack = mock(DcMotor.class);
-        rightBack = mock(DcMotor.class);
-
-        intake = mock(DcMotor.class);
-        Launcher = mock(DcMotor.class);
-        Launcher2 = mock(DcMotor.class);
-
-        rampServo1 = mock(CRServo.class);
-        rampServo2 = mock(CRServo.class);
-        rampServo3 = mock(CRServo.class);
-        rampServo4 = mock(CRServo.class);
-
-
-        // Mock HardwareMap and its nested dcMotor map
-        hardwareMap = mock(HardwareMap.class);
-        HardwareMap.DeviceMapping<CRServo> crServoMap = mock(HardwareMap.DeviceMapping.class);
-        HardwareMap.DeviceMapping<DcMotor> dcMotorMap = mock(HardwareMap.DeviceMapping.class);
-
-        when(hardwareMap.get(DcMotor.class, "intake")).thenReturn(intake);
-        when(hardwareMap.get(DcMotor.class, "leftFront")).thenReturn(leftFront);
-        when(hardwareMap.get(DcMotor.class, "rightFront")).thenReturn(rightFront);
-        when(hardwareMap.get(DcMotor.class, "leftBack")).thenReturn(leftBack);
-        when(hardwareMap.get(DcMotor.class, "rightBack")).thenReturn(rightBack);
-
-        when(dcMotorMap.get("leftFront")).thenReturn(leftFront);
-        when(dcMotorMap.get("rightFront")).thenReturn(rightFront);
-        when(dcMotorMap.get("leftBack")).thenReturn(leftBack);
-        when(dcMotorMap.get("rightBack")).thenReturn(rightBack);
-        when(dcMotorMap.get("Launcher")).thenReturn(Launcher);
-        when(dcMotorMap.get("Launcher2")).thenReturn(Launcher2);
-        when(crServoMap.get("rampServo1")).thenReturn(rampServo1);
-        when(crServoMap.get("rampServo2")).thenReturn(rampServo2);
-        when(crServoMap.get("rampServo3")).thenReturn(rampServo3);
-        when(crServoMap.get("rampServo4")).thenReturn(rampServo4);
-
-        // Set direction of the main drive motors
-        leftFront.setDirection(DcMotorSimple.Direction.FORWARD);
-        rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftBack.setDirection(DcMotorSimple.Direction.FORWARD);
-        rightBack.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        // Set the direction of the intake motor
-        intake.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        hardwareMap.dcMotor = dcMotorMap;
-        hardwareMap.crservo = crServoMap;
-        controls = new CommonControls(hardwareMap);
-    }
-
-    @DisplayName("Pushing left stick Y fully should set all motors to 1.0")
-    @Test
-    void testSetDrivePower_forwardOnly() {
-        // Forward input only, no strafe or rotation, full speed
-        controls.setDrivePower(-1.0f, 0.0f, 0.0f,
-            0.0f);
-
-        // Expected power: all motors get 1.0
-        verify(leftFront).setPower(-1.0);
-        verify(rightFront).setPower(-1.0);
-        verify(leftBack).setPower(-1.0);
-        verify(rightBack).setPower(-1.0);
-    }
-
-    @DisplayName("Pushing left stick Y fully should set back right and front left motors to 1.0" +
-        "and should set back left and front right motors to -1.0")
-    @Test
-    void testSetDrivePower_strafeRightOnly() {
-        controls.setDrivePower(0.0f, -1.0f, 0.0f,
-            0.0f);
-
-        // Expected power: set back right and front left motors to 1.0 and should set back left and
-        // front right motors to -1.0"
-        // Expected power: all motors get 1.0
-        verify(leftFront).setPower(1.0);
-        verify(rightFront).setPower(-1.0);
-        verify(leftBack).setPower(-1.0);
-        verify(rightBack).setPower(1.0);
-    }
-
-    @DisplayName("Pushing right stick X fully should set the left motors to 1.0" +
-        " and the right motors to -1.0")
-    @Test
-    void testSetDrivePower_turnRight() {
-        controls.setDrivePower(0.0f, 0.0f, -1.0f,
-            0.0f);
-
-        // Expected power: left motors get 1.0 and right motors get -1.0
-        verify(leftFront).setPower(1.0);
-        verify(rightFront).setPower(-1.0);
-        verify(leftBack).setPower(1.0);
-        verify(rightBack).setPower(-1.0);
-    }
-
-    @DisplayName("Intake should spin forward when forward button is pressed")
-    @Test
-    void testIntakeForward() {
-        controls.setIntakeDirection(true, false);
-        verify(intake).setPower(1.0);
-        verify(rampServo1).setPower(-1.0);
-        verify(rampServo2).setPower(1.0);
-    }
-
-    @DisplayName("Intake should spin backward when reverse button is pressed")
-    @Test
-    void testIntakeReverse() {
-        controls.setIntakeDirection(false, true);
-        verify(intake).setPower(-1.0);
-        verify(rampServo1).setPower(1.0);
-        verify(rampServo2).setPower(-1.0);
-    }
-
-    @DisplayName("Intake should not spin when direction buttons are not depressed")
-    @Test
-    void testIntakeNeutral() {
-        controls.setIntakeDirection(false, false);
-        verify(intake).setPower(0.0);
-        verify(rampServo1).setPower(-0.0);
-        verify(rampServo2).setPower(0.0);
-    }
-
-    @DisplayName("Intake should stop when both direction buttons pressed at the same time")
-    @Test
-    void testIntakeConflict() {
-        controls.setIntakeDirection(true, true);
-        verify(intake).setPower(0.0);
-        verify(rampServo1).setPower(-0.0);
-        verify(rampServo2).setPower(0.0);
-    }
-
-    @DisplayName("Launcher should spin when input given")
-    @Test
-    void testLaunchPower() {
-        controls.setLaunchPower(true);
-        verify(Launcher).setPower(-1.0);
-        verify(Launcher2).setPower(1.0);
-    }
-
-    @DisplayName("Launcher should stop when no input it given")
-    @Test
-    void testNoLaunchPower() {
-        controls.setLaunchPower(false);
-        verify(Launcher).setPower(-0.0);
-        verify(Launcher2).setPower(0.0);
-    }
-
-    @DisplayName("Set zero power behavior to brake when true")
-    @Test
-    void testDriveBrakeActive() {
-        controls.setDriveMotorZeroPowerBehavior(true);
-        verify(leftFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        verify(rightFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        verify(leftBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        verify(rightBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-    }
-
-    @DisplayName("Set zero power behavior to float when false")
-    @Test
-    void testDriveBrakeInactive() {
-        controls.setDriveMotorZeroPowerBehavior(false);
-        verify(leftFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        verify(rightFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        verify(leftBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-        verify(rightBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-    }
-}
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
new file mode 100644
index 0000000..0228c8d
--- /dev/null
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
@@ -0,0 +1,96 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.Utils_13233.CommonControls;
+import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
+import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.DisplayName;
+import org.junit.jupiter.api.Test;
+import org.mockito.MockitoAnnotations;
+
+public class DriveControlsTest {
+    public DriveControls driveControls;
+
+    MockMotorUtil mockMotor = new MockMotorUtil();
+
+    @BeforeEach
+    void motorSetup(){
+        mockMotor.setUp();
+        driveControls = new DriveControls(mockMotor.hardwareMap);
+    }
+
+
+
+    @DisplayName("Pushing left stick Y fully should set all motors to 1.0")
+    @Test
+    void testSetDrivePower_forwardOnly() {
+        // Forward input only, no strafe or rotation, full speed
+        driveControls.setDrivePower(-1.0f, 0.0f, 0.0f,
+            0.0f);
+
+        // Expected power: all motors get 1.0
+        verify(mockMotor.leftFront).setPower(-1.0);
+        verify(mockMotor.rightFront).setPower(-1.0);
+        verify(mockMotor.leftBack).setPower(-1.0);
+        verify(mockMotor.rightBack).setPower(-1.0);
+    }
+
+    @DisplayName("Pushing left stick Y fully should set back right and front left motors to 1.0" +
+        "and should set back left and front right motors to -1.0")
+    @Test
+    void testSetDrivePower_strafeRightOnly() {
+        driveControls.setDrivePower(0.0f, -1.0f, 0.0f,
+            0.0f);
+
+        // Expected power: set back right and front left motors to 1.0 and should set back left and
+        // front right motors to -1.0"
+        // Expected power: all motors get 1.0
+        verify(mockMotor.leftFront).setPower(1.0);
+        verify(mockMotor.rightFront).setPower(-1.0);
+        verify(mockMotor.leftBack).setPower(-1.0);
+        verify(mockMotor.rightBack).setPower(1.0);
+    }
+
+    @DisplayName("Pushing right stick X fully should set the left motors to 1.0" +
+        " and the right motors to -1.0")
+    @Test    void testSetDrivePower_turnRight() {
+        driveControls.setDrivePower(0.0f, 0.0f, -1.0f,
+            0.0f);
+
+        // Expected power: left motors get 1.0 and right motors get -1.0
+        verify(mockMotor.leftFront).setPower(1.0);
+        verify(mockMotor.rightFront).setPower(-1.0);
+        verify(mockMotor.leftBack).setPower(1.0);
+        verify(mockMotor.rightBack).setPower(-1.0);
+    }
+
+    @DisplayName("Set zero power behavior to brake when true")
+    @Test
+    void testDriveBrakeActive() {
+        driveControls.setDriveMotorZeroPowerBehavior(true);
+        verify(mockMotor.leftFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        verify(mockMotor.rightFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        verify(mockMotor.leftBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        verify(mockMotor.rightBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    @DisplayName("Set zero power behavior to float when false")
+    @Test
+    void testDriveBrakeInactive() {
+        driveControls.setDriveMotorZeroPowerBehavior(false);
+        verify(mockMotor.leftFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        verify(mockMotor.rightFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        verify(mockMotor.leftBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        verify(mockMotor.rightBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+    }
+}
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java
new file mode 100644
index 0000000..8f04fd1
--- /dev/null
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java
@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.MockMotorUtil;
+import org.firstinspires.ftc.teamcode.Utils_13233.CommonControls;
+import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
+import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.DisplayName;
+import org.junit.jupiter.api.Test;
+import org.mockito.MockitoAnnotations;
+public class LaunchControlsTest {
+    public LaunchControls launchControls;
+
+    MockMotorUtil mockMotor = new MockMotorUtil();
+
+    @BeforeEach
+    void motorSetup(){
+        mockMotor.setUp();
+        launchControls = new LaunchControls(mockMotor.hardwareMap);
+    }
+
+    @DisplayName("Launcher should spin when input given")
+    @Test
+    void testLaunchPower() {
+        launchControls.setLaunchPower(true);
+        verify(mockMotor.launcher).setPower(-1.0);
+        verify(mockMotor.launcher2).setPower(1.0);
+    }
+
+    @DisplayName("Launcher should stop when no input it given")
+    @Test
+    void testNoLaunchPower() {
+        launchControls.setLaunchPower(false);
+        verify(mockMotor.launcher).setPower(-0.0);
+        verify(mockMotor.launcher2).setPower(0.0);
+    }
+}
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java
new file mode 100644
index 0000000..fdf6ab7
--- /dev/null
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java
@@ -0,0 +1,56 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.when;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
+import org.junit.jupiter.api.BeforeEach;
+import org.mockito.MockitoAnnotations;
+
+public class MockMotorUtil {
+    public DcMotor launcher, launcher2, intake, rightFront, leftFront, leftBack, rightBack;
+    public CRServo rampServo1, rampServo2, rampServo3, rampServo4;
+    public HardwareMap hardwareMap;
+
+
+    public void setUp() {
+        MockitoAnnotations.openMocks(this);
+
+        hardwareMap = mock(HardwareMap.class);
+
+        intake = mock(DcMotor.class);
+
+        leftFront = mock(DcMotor.class);
+        rightFront = mock(DcMotor.class);
+        leftBack = mock(DcMotor.class);
+        rightBack = mock(DcMotor.class);
+
+        launcher = mock(DcMotor.class);
+        launcher2 = mock(DcMotor.class);
+
+        rampServo1 = mock(CRServo.class);
+        rampServo2 = mock(CRServo.class);
+        rampServo3 = mock(CRServo.class);
+        rampServo4 = mock(CRServo.class);
+
+
+        when(hardwareMap.get(DcMotor.class, "intake")).thenReturn(intake);
+
+        when(hardwareMap.get(DcMotor.class, "leftFront")).thenReturn(leftFront);
+        when(hardwareMap.get(DcMotor.class, "rightFront")).thenReturn(rightFront);
+        when(hardwareMap.get(DcMotor.class, "leftBack")).thenReturn(leftBack);
+        when(hardwareMap.get(DcMotor.class, "rightBack")).thenReturn(rightBack);
+
+        when(hardwareMap.get(DcMotor.class, "Launcher")).thenReturn(launcher);
+        when(hardwareMap.get(DcMotor.class, "Launcher2")).thenReturn(launcher2);
+
+        when(hardwareMap.get(CRServo.class, "rampServo1")).thenReturn(rampServo1);
+        when(hardwareMap.get(CRServo.class, "rampServo2")).thenReturn(rampServo2);
+        when(hardwareMap.get(CRServo.class, "rampServo3")).thenReturn(rampServo3);
+        when(hardwareMap.get(CRServo.class, "rampServo4")).thenReturn(rampServo4);
+    }
+}
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java
new file mode 100644
index 0000000..044bc3f
--- /dev/null
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java
@@ -0,0 +1,66 @@
+package org.firstinspires.ftc.teamcode;
+
+import org.firstinspires.ftc.teamcode.Utils_13233.RampControls;
+import org.junit.jupiter.api.BeforeEach;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.junit.jupiter.api.DisplayName;
+import org.junit.jupiter.api.Test;
+
+
+public class RampControlsTest {
+    public RampControls rampControls;
+
+    MockMotorUtil mockMotor = new MockMotorUtil();
+
+    @BeforeEach
+    void motorSetup(){
+        mockMotor.setUp();
+        rampControls = new RampControls(mockMotor.hardwareMap);
+    }
+
+    @DisplayName("Intake should spin forward when forward button is pressed")
+    @Test
+    void testIntakeForward() {
+        rampControls.setIntakeDirection(true, false);
+        verify(mockMotor.intake).setPower(1.0);
+        verify(mockMotor.rampServo1).setPower(-1.0);
+        verify(mockMotor.rampServo2).setPower(1.0);
+    }
+
+    @DisplayName("Intake should spin backward when reverse button is pressed")
+    @Test
+    void testIntakeReverse() {
+        rampControls.setIntakeDirection(false, true);
+        verify(mockMotor.intake).setPower(-1.0);
+        verify(mockMotor.rampServo1).setPower(1.0);
+        verify(mockMotor.rampServo2).setPower(-1.0);
+    }
+
+    @DisplayName("Intake should not spin when direction buttons are not depressed")
+    @Test
+    void testIntakeNeutral() {
+        rampControls.setIntakeDirection(false, false);
+        verify(mockMotor.intake).setPower(0.0);
+        verify(mockMotor.rampServo1).setPower(-0.0);
+        verify(mockMotor.rampServo2).setPower(0.0);
+    }
+
+    @DisplayName("Intake should stop when both direction buttons pressed at the same time")
+    @Test
+    void testIntakeConflict() {
+        rampControls.setIntakeDirection(true, true);
+        verify(mockMotor.intake).setPower(0.0);
+        verify(mockMotor.rampServo1).setPower(-0.0);
+        verify(mockMotor.rampServo2).setPower(0.0);
+    }
+}

From a0997db0a9b7025122aadabce2742b379ce5dd7d Mon Sep 17 00:00:00 2001
From: Jeffrey Mello <mrjeffreymello@gmail.com>
Date: Sat, 15 Nov 2025 21:56:44 -0500
Subject: [PATCH 2/2] Refactor CommonControls and update tests

Moved CommonControls to main package, removed old Utils_13233/CommonControls, and added comprehensive unit tests for CommonControls. Updated TeleOpMain to use the new CommonControls implementation and cleaned up test imports for DriveControlsTest and LaunchControlsTest.
---
 .../ftc/teamcode/CommonControls.java          | 191 +++++++++++++++++
 .../ftc/teamcode/TeleOpMain.java              |   7 +-
 .../teamcode/Utils_13233/CommonControls.java  |  20 --
 .../ftc/teamcode/CommonControlsTest.java      | 192 ++++++++++++++++++
 .../ftc/teamcode/DriveControlsTest.java       |   7 -
 .../ftc/teamcode/LaunchControlsTest.java      |  11 +-
 6 files changed, 390 insertions(+), 38 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java
 create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java
new file mode 100644
index 0000000..1be2124
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java
@@ -0,0 +1,191 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+
+/**
+ * Class the holds common functions and variable for the 13233 DECODE robot
+ */
+public class CommonControls {
+    /**
+     * Front left drive motor
+     */
+    public DcMotor leftFront;
+    /**
+     * Front right drive motor
+     */
+    public DcMotor rightFront;
+    /**
+     * Back left drive motor
+     */
+    public DcMotor leftBack;
+    /**
+     * Right back drive motor
+     */
+    public DcMotor rightBack;
+    public float launchPower = 1.0f;
+    /**
+     * The motor for that controls the ball intake
+     */
+    private DcMotor intake;
+    /**
+     * One of the motors used to shoot the balls
+     */
+    private DcMotor Launcher;
+    /**
+     * One of the motors used to shoot the balls
+     */
+    private DcMotor Launcher2;
+    /**
+     * One of the servos used to carry the balls up the ramp
+     */
+    private CRServo rampServo1;
+    /**
+     * One of the servos used to carry the balls up the ramp
+     */
+    private CRServo rampServo2;
+    private CRServo rampServo3;
+    private CRServo rampServo4;
+
+    /**
+     * Constructor for the CommonControls class
+     *
+     * @param hardwareMap The hardware map for the robot
+     */
+    public CommonControls(HardwareMap hardwareMap) {
+        // Map main Drive Motors
+        leftFront = hardwareMap.get(DcMotor.class, "leftFront");
+        rightFront = hardwareMap.get(DcMotor.class, "rightFront");
+        leftBack = hardwareMap.get(DcMotor.class, "leftBack");
+        rightBack = hardwareMap.get(DcMotor.class, "rightBack");
+
+        // Map Intake Motor
+        intake = hardwareMap.get(DcMotor.class, "intake");
+
+        // Map the launcher Prototype
+        Launcher = hardwareMap.dcMotor.get("Launcher");
+        Launcher2 = hardwareMap.dcMotor.get("Launcher2");
+        rampServo1 = hardwareMap.crservo.get("rampServo1");
+        rampServo2 = hardwareMap.crservo.get("rampServo2");
+        rampServo3 = hardwareMap.crservo.get("rampServo3");
+        rampServo4 = hardwareMap.crservo.get("rampServo4");
+        // Set direction of the main drive motors
+        leftFront.setDirection(DcMotorSimple.Direction.FORWARD);
+        rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftBack.setDirection(DcMotorSimple.Direction.FORWARD);
+        rightBack.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // Set the direction of the intake motor
+        intake.setDirection(DcMotorSimple.Direction.REVERSE);
+    }
+
+
+    // TODO:
+    //        Implement field oriented drive
+
+    /**
+     * Updates all the drive controls based on the current gamepad stick positions
+     *
+     * @param controlLeftStickY Left stick Y setting, up/down
+     * @param controlLeftStickX Left stick X setting, left/right
+     * @param controlRightStick Right Stick setting, controls left right turn
+     * @param speedLimiter      Trigger setting, controls speed of the robot
+     */
+    public void setDrivePower(float controlLeftStickY, float controlLeftStickX,
+                              float controlRightStick, float speedLimiter) {
+        // For readability and flexibility for future control input changes
+        //noinspection UnnecessaryLocalVariable
+        float forward = controlLeftStickY;
+        //noinspection UnnecessaryLocalVariable
+        float strafe = controlLeftStickX;
+        //noinspection UnnecessaryLocalVariable
+        float rotate = controlRightStick;
+        float speed = Math.max(1.0f - speedLimiter, 0.5f);
+
+
+        // Calculate the power for each motor
+        // Remember the direction of the stick is the opposite of what you would expect
+        // up is in the negatives and down is in the positives
+        float frontRightPower = (forward + strafe + rotate) * (speed);
+        float frontLeftPower = (forward - strafe - rotate) * (speed);
+        float backRightPower = (forward - strafe + rotate) * (speed);
+        float backLeftPower = (forward + strafe - rotate) * (speed);
+
+        // Set the power for each of the motors
+        rightFront.setPower(frontRightPower);
+        leftFront.setPower(frontLeftPower);
+        rightBack.setPower(backRightPower);
+        leftBack.setPower(backLeftPower);
+    }
+
+    /**
+     * Updates just the forward and back movement based on controller stick position
+     *
+     * @param controlLeftStickY Left stick Y setting, up/down
+     */
+    public void setDrivePower(float controlLeftStickY) {
+        setDrivePower(controlLeftStickY, 0.0f, 0.0f, 0.0f);
+    }
+
+
+    /**
+     * Sets the power of all of the launch motors depending on the value of launchInput
+     *
+     * @param launchInput Button mapped to launch input
+     */
+    void setLaunchPower(boolean launchInput) {
+        double power = launchInput ? launchPower : 0.0;
+
+        // Set the the power value to the motors
+        Launcher.setPower(power);
+        Launcher2.setPower(-power);
+    }
+
+    /**
+     * Controls the spin direction of the intake wheel based on controller buttons
+     *
+     * @param intakeForwardInput Button mapped to forward input
+     * @param intakeReverseInput Button mapped to reverse input
+     */
+    void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverseInput) {
+        double intakePower = 0;
+        // ^ is the XOR operator, will return true if only one variable is true
+        // If intakeForwardInput OR intakeReverseInput is true then this is true, but not
+        // if both are true, which is why this differs from the || logical or operator
+        if (intakeForwardInput ^ intakeReverseInput) {
+            // This uses an inline-if statement which is useful when assigning values to variables
+            // This says set power = 1 if intakeForwardInput is true, else set it to -1
+            intakePower = intakeForwardInput ? 1 : -1;
+        }
+        rampServo1.setPower(-intakePower);
+        rampServo2.setPower(intakePower);
+        rampServo3.setPower(intakePower);
+        rampServo4.setPower(-intakePower);
+        intake.setPower(intakePower);
+    }
+
+    void setDriveBrake() {
+        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    void setDriveFloat() {
+        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+    }
+
+    void setDriveMotorZeroPowerBehavior(boolean input) {
+        if (input) {
+            setDriveBrake();
+        } else {
+            setDriveFloat();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
index c95d742..36e55c2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
@@ -44,8 +44,13 @@ public void runOpMode() {
             // Set the power to the launch motors based while the x button is being pressed
             launch.setLaunchPower(gamepad1.x);
 
+<<<<<<< HEAD
             //Add option to enable brakes when sharbell holds a
             drive.setDriveMotorZeroPowerBehavior(gamepad1.a);
+=======
+            //Add option to enable brakes when driver 1 holds the "a" button
+            CommonControls.setDriveMotorZeroPowerBehavior(gamepad1.a);
+>>>>>>> main
 
             // Set all of the intake motors and servos to go forward when dpad up is pressed
             // and reverse when dpad down is pressed
@@ -58,8 +63,8 @@ public void runOpMode() {
             // Sets the power to the drive motors based on current gamepad inputs
             drive.setDrivePower(gamepad1.left_stick_y, gamepad1.left_stick_x,
                 gamepad1.right_stick_x, gamepad1.left_trigger);
+            sleep(50);
         }
-        sleep(50);
         // Do not place any drive code here outside of the while loop (will fail inspection)
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java
deleted file mode 100644
index 393b1fd..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonControls.java
+++ /dev/null
@@ -1,20 +0,0 @@
-package org.firstinspires.ftc.teamcode.Utils_13233;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-
-/**
- * Class the holds common functions and variable for the 13233 DECODE robot
- */
-public class CommonControls{
-    private final MotorConstructor motors;
-
-    public CommonControls(HardwareMap hardwareMap) {
-        this.motors = new MotorConstructor(hardwareMap);
-    }
-
-
-
-
-}
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java
new file mode 100644
index 0000000..02e040e
--- /dev/null
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/CommonControlsTest.java
@@ -0,0 +1,192 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.DisplayName;
+import org.junit.jupiter.api.Test;
+
+public class CommonControlsTest {
+
+    private DcMotor leftFront, rightFront, leftBack, rightBack, intake, Launcher, Launcher2;
+    private CRServo rampServo1;
+    private CRServo rampServo2;
+    private CommonControls controls;
+
+    @BeforeEach
+    void setUp() {
+        // Mock all motors
+        leftFront = mock(DcMotor.class);
+        rightFront = mock(DcMotor.class);
+        leftBack = mock(DcMotor.class);
+        rightBack = mock(DcMotor.class);
+
+        intake = mock(DcMotor.class);
+        Launcher = mock(DcMotor.class);
+        Launcher2 = mock(DcMotor.class);
+
+        rampServo1 = mock(CRServo.class);
+        rampServo2 = mock(CRServo.class);
+        CRServo rampServo3 = mock(CRServo.class);
+        CRServo rampServo4 = mock(CRServo.class);
+
+
+        // Mock HardwareMap and its nested dcMotor map
+        HardwareMap hardwareMap = mock(HardwareMap.class);
+        HardwareMap.DeviceMapping<CRServo> crServoMap = mock(HardwareMap.DeviceMapping.class);
+        HardwareMap.DeviceMapping<DcMotor> dcMotorMap = mock(HardwareMap.DeviceMapping.class);
+
+        when(hardwareMap.get(DcMotor.class, "intake")).thenReturn(intake);
+        when(hardwareMap.get(DcMotor.class, "leftFront")).thenReturn(leftFront);
+        when(hardwareMap.get(DcMotor.class, "rightFront")).thenReturn(rightFront);
+        when(hardwareMap.get(DcMotor.class, "leftBack")).thenReturn(leftBack);
+        when(hardwareMap.get(DcMotor.class, "rightBack")).thenReturn(rightBack);
+
+        when(dcMotorMap.get("leftFront")).thenReturn(leftFront);
+        when(dcMotorMap.get("rightFront")).thenReturn(rightFront);
+        when(dcMotorMap.get("leftBack")).thenReturn(leftBack);
+        when(dcMotorMap.get("rightBack")).thenReturn(rightBack);
+        when(dcMotorMap.get("Launcher")).thenReturn(Launcher);
+        when(dcMotorMap.get("Launcher2")).thenReturn(Launcher2);
+        when(crServoMap.get("rampServo1")).thenReturn(rampServo1);
+        when(crServoMap.get("rampServo2")).thenReturn(rampServo2);
+        when(crServoMap.get("rampServo3")).thenReturn(rampServo3);
+        when(crServoMap.get("rampServo4")).thenReturn(rampServo4);
+
+        // Set direction of the main drive motors
+        leftFront.setDirection(DcMotorSimple.Direction.FORWARD);
+        rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftBack.setDirection(DcMotorSimple.Direction.FORWARD);
+        rightBack.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // Set the direction of the intake motor
+        intake.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        hardwareMap.dcMotor = dcMotorMap;
+        hardwareMap.crservo = crServoMap;
+        controls = new CommonControls(hardwareMap);
+    }
+
+    @DisplayName("Pushing left stick Y fully should set all motors to 1.0")
+    @Test
+    void testSetDrivePower_forwardOnly() {
+        // Forward input only, no strafe or rotation, full speed
+        controls.setDrivePower(-1.0f, 0.0f, 0.0f,
+            0.0f);
+
+        // Expected power: all motors get 1.0
+        verify(leftFront).setPower(-1.0);
+        verify(rightFront).setPower(-1.0);
+        verify(leftBack).setPower(-1.0);
+        verify(rightBack).setPower(-1.0);
+    }
+
+    @DisplayName("Pushing left stick Y fully should set back right and front left motors to 1.0" +
+        "and should set back left and front right motors to -1.0")
+    @Test
+    void testSetDrivePower_strafeRightOnly() {
+        controls.setDrivePower(0.0f, -1.0f, 0.0f,
+            0.0f);
+
+        // Expected power: set back right and front left motors to 1.0 and should set back left and
+        // front right motors to -1.0"
+        // Expected power: all motors get 1.0
+        verify(leftFront).setPower(1.0);
+        verify(rightFront).setPower(-1.0);
+        verify(leftBack).setPower(-1.0);
+        verify(rightBack).setPower(1.0);
+    }
+
+    @DisplayName("Pushing right stick X fully should set the left motors to 1.0" +
+        " and the right motors to -1.0")
+    @Test
+    void testSetDrivePower_turnRight() {
+        controls.setDrivePower(0.0f, 0.0f, -1.0f,
+            0.0f);
+
+        // Expected power: left motors get 1.0 and right motors get -1.0
+        verify(leftFront).setPower(1.0);
+        verify(rightFront).setPower(-1.0);
+        verify(leftBack).setPower(1.0);
+        verify(rightBack).setPower(-1.0);
+    }
+
+    @DisplayName("Intake should spin forward when forward button is pressed")
+    @Test
+    void testIntakeForward() {
+        controls.setIntakeDirection(true, false);
+        verify(intake).setPower(1.0);
+        verify(rampServo1).setPower(-1.0);
+        verify(rampServo2).setPower(1.0);
+    }
+
+    @DisplayName("Intake should spin backward when reverse button is pressed")
+    @Test
+    void testIntakeReverse() {
+        controls.setIntakeDirection(false, true);
+        verify(intake).setPower(-1.0);
+        verify(rampServo1).setPower(1.0);
+        verify(rampServo2).setPower(-1.0);
+    }
+
+    @DisplayName("Intake should not spin when direction buttons are not depressed")
+    @Test
+    void testIntakeNeutral() {
+        controls.setIntakeDirection(false, false);
+        verify(intake).setPower(0.0);
+        verify(rampServo1).setPower(-0.0);
+        verify(rampServo2).setPower(0.0);
+    }
+
+    @DisplayName("Intake should stop when both direction buttons pressed at the same time")
+    @Test
+    void testIntakeConflict() {
+        controls.setIntakeDirection(true, true);
+        verify(intake).setPower(0.0);
+        verify(rampServo1).setPower(-0.0);
+        verify(rampServo2).setPower(0.0);
+    }
+
+    @DisplayName("Launcher should spin when input given")
+    @Test
+    void testLaunchPower() {
+        controls.setLaunchPower(true);
+        verify(Launcher).setPower(controls.launchPower);
+        verify(Launcher2).setPower(-controls.launchPower);
+    }
+
+    @DisplayName("Launcher should stop when no input it given")
+    @Test
+    void testNoLaunchPower() {
+        controls.setLaunchPower(false);
+        verify(Launcher).setPower(0.0);
+        verify(Launcher2).setPower(-0.0);
+    }
+
+    @DisplayName("Set zero power behavior to brake when true")
+    @Test
+    void testDriveBrakeActive() {
+        controls.setDriveMotorZeroPowerBehavior(true);
+        verify(leftFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        verify(rightFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        verify(leftBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        verify(rightBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    @DisplayName("Set zero power behavior to float when false")
+    @Test
+    void testDriveBrakeInactive() {
+        controls.setDriveMotorZeroPowerBehavior(false);
+        verify(leftFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        verify(rightFront).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        verify(leftBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        verify(rightBack).setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+    }
+}
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
index 0228c8d..65d4946 100644
--- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
@@ -3,20 +3,13 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.mockito.Mockito.mock;
 import static org.mockito.Mockito.verify;
-import static org.mockito.Mockito.when;
 
-import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.Utils_13233.CommonControls;
 import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
-import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.DisplayName;
 import org.junit.jupiter.api.Test;
-import org.mockito.MockitoAnnotations;
 
 public class DriveControlsTest {
     public DriveControls driveControls;
diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java
index 8f04fd1..9f52702 100644
--- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java
@@ -3,21 +3,12 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.mockito.Mockito.mock;
 import static org.mockito.Mockito.verify;
-import static org.mockito.Mockito.when;
 
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.teamcode.MockMotorUtil;
-import org.firstinspires.ftc.teamcode.Utils_13233.CommonControls;
-import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
 import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.DisplayName;
 import org.junit.jupiter.api.Test;
-import org.mockito.MockitoAnnotations;
+
 public class LaunchControlsTest {
     public LaunchControls launchControls;