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/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 5fc74a2..2ff114c 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,23 @@ 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
+            drive.setDriveMotorZeroPowerBehavior(gamepad1.a);
             //Add option to enable brakes when driver 1 holds the "a" button
-            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/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/DriveControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
new file mode 100644
index 0000000..65d4946
--- /dev/null
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java
@@ -0,0 +1,89 @@
+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 com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.DisplayName;
+import org.junit.jupiter.api.Test;
+
+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..9f52702
--- /dev/null
+++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/LaunchControlsTest.java
@@ -0,0 +1,38 @@
+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 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;
+
+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);
+    }
+}