From b6b60b7bf51a28dd0e15abb66a1e2e4055100806 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Thu, 9 Feb 2023 22:01:08 -0500
Subject: [PATCH 01/43] added roadrunner

---
 .idea/jarRepositories.xml                     |   5 +
 TeamCode/build.gradle                         |   3 +
 .../ftc/teamcode/auto/AutoLeft.java           | 153 ----
 .../ftc/teamcode/auto/AutoTest.java           | 199 -----
 .../ftc/teamcode/drive/DriveConstants.java    | 120 +++
 .../teamcode/drive/SampleMecanumDrive.java    | 297 ++++++++
 .../ftc/teamcode/drive/SampleTankDrive.java   | 304 ++++++++
 .../drive/StandardTrackingWheelLocalizer.java |  80 ++
 .../ftc/teamcode/old/Auto/BOEcode.java        | 114 ---
 .../old/Auto/BetterRobotAutonomous.java       |  91 ---
 .../ftc/teamcode/old/Auto/NewBotAutoLeft.java | 137 ----
 .../teamcode/old/Auto/NewBotAutoRight.java    | 147 ----
 .../teamcode/old/Auto/OldRobotAutonomous.java |  49 --
 .../old/Auto/OldRobotAutonomousOther.java     |  50 --
 .../teamcode/old/Auto/StrafeBotAutoLeft.java  |  55 --
 .../ftc/teamcode/{ => old}/RobotOpMode.java   |   6 +-
 .../ftc/teamcode/old/Robots/NewRobot.java     |  84 ---
 .../ftc/teamcode/old/Robots/OldRobot.java     |  83 --
 .../old/Subsystems/BackTankDrive.java         |  79 --
 .../teamcode/old/Subsystems/BoeDetection.java | 185 -----
 .../old/Subsystems/FullDetection.java         | 170 -----
 .../teamcode/old/Subsystems/LinearSlide.java  | 102 ---
 .../old/Subsystems/NewPoleDetection.java      |  84 ---
 .../old/Subsystems/PoleDetection.java         | 100 ---
 .../old/Subsystems/SingleJointGripperArm.java | 102 ---
 .../old/Subsystems/SleeveDetection.java       | 134 ----
 .../teamcode/old/Subsystems/StrafeDrive.java  | 176 -----
 .../old/TeleOp/CameraRobotTeleOp.java         |  80 --
 .../teamcode/old/TeleOp/NewRobotTeleOp.java   |  26 -
 .../teamcode/old/TeleOp/OldRobotTeleOp.java   |  22 -
 .../ftc/teamcode/old/Util/Maths.java          |  30 -
 .../ftc/teamcode/old/Util/Measure.java        |  12 -
 .../teamcode/{ => old}/auto/AutoRight.java    |  14 +-
 .../{ => old}/auto/AutoTestRight.java         |  16 +-
 .../{ => old}/pipeline/BoeDetection.java      |   2 +-
 .../{ => old}/pipeline/PoleDetection.java     |   2 +-
 .../pipeline/SleeveDetectionNew.java          |   2 +-
 .../pipeline/SleeveDetectionRight.java        |   2 +-
 .../{ => old}/subsystem/LinearSlide.java      |   4 +-
 .../{ => old}/subsystem/StrafeDrive.java      |   4 +-
 .../{ => old}/subsystem/Subsystem.java        |   4 +-
 .../teamcode/{ => old}/subsystem/Webcam.java  |   4 +-
 .../ftc/teamcode/old/teleop/SensorColor.java  | 226 ++++++
 .../opmode/AutomaticFeedforwardTuner.java     | 221 ++++++
 .../ftc/teamcode/opmode/BackAndForth.java     |  52 ++
 .../opmode/DriveVelocityPIDTuner.java         | 171 +++++
 .../ftc/teamcode/opmode/FollowerPIDTuner.java |  55 ++
 .../ftc/teamcode/opmode/LocalizationTest.java |  45 ++
 .../opmode/ManualFeedforwardTuner.java        | 147 ++++
 .../teamcode/opmode/MaxAngularVeloTuner.java  |  73 ++
 .../ftc/teamcode/opmode/MaxVelocityTuner.java |  84 +++
 .../opmode/MotorDirectionDebugger.java        |  93 +++
 .../ftc/teamcode/opmode/SplineTest.java       |  38 +
 .../ftc/teamcode/opmode/StrafeTest.java       |  46 ++
 .../ftc/teamcode/opmode/StraightTest.java     |  46 ++
 .../ftc/teamcode/opmode/TrackWidthTuner.java  |  88 +++
 .../TrackingWheelForwardOffsetTuner.java      | 104 +++
 .../TrackingWheelLateralDistanceTuner.java    | 130 ++++
 .../ftc/teamcode/opmode/TurnTest.java         |  27 +
 .../org/firstinspires/ftc/teamcode/readme.md  | 132 +++-
 .../ftc/teamcode/teleop/SensorColor.java      | 222 ------
 .../ftc/teamcode/teleop/TwoPlayerTeleop.java  |  27 -
 .../EmptySequenceException.java               |   4 +
 .../TrajectorySequence.java                   |  44 ++
 .../TrajectorySequenceBuilder.java            | 711 ++++++++++++++++++
 .../TrajectorySequenceRunner.java             | 279 +++++++
 .../sequencesegment/SequenceSegment.java      |  40 +
 .../sequencesegment/TrajectorySegment.java    |  20 +
 .../sequencesegment/TurnSegment.java          |  36 +
 .../sequencesegment/WaitSegment.java          |  12 +
 .../util/AssetsTrajectoryManager.java         |  70 ++
 .../ftc/teamcode/util/AxesSigns.java          |  45 ++
 .../ftc/teamcode/util/AxisDirection.java      |   8 +
 .../ftc/teamcode/util/DashboardUtil.java      |  54 ++
 .../ftc/teamcode/util/Encoder.java            | 125 +++
 .../ftc/teamcode/util/LogFiles.java           | 226 ++++++
 .../ftc/teamcode/util/LoggingUtil.java        |  60 ++
 .../ftc/teamcode/util/LynxModuleUtil.java     | 124 +++
 .../ftc/teamcode/util/RegressionUtil.java     | 156 ++++
 build.dependencies.gradle                     |   2 +
 80 files changed, 4630 insertions(+), 2746 deletions(-)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BOEcode.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BetterRobotAutonomous.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoLeft.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoRight.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomous.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomousOther.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/StrafeBotAutoLeft.java
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/RobotOpMode.java (84%)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/NewRobot.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/OldRobot.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BackTankDrive.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BoeDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/FullDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/LinearSlide.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/NewPoleDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/PoleDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SingleJointGripperArm.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SleeveDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/StrafeDrive.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/CameraRobotTeleOp.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/NewRobotTeleOp.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/OldRobotTeleOp.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Maths.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Measure.java
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/auto/AutoRight.java (91%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/auto/AutoTestRight.java (92%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/pipeline/BoeDetection.java (99%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/pipeline/PoleDetection.java (98%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/pipeline/SleeveDetectionNew.java (98%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/pipeline/SleeveDetectionRight.java (98%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/subsystem/LinearSlide.java (97%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/subsystem/StrafeDrive.java (99%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/subsystem/Subsystem.java (63%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => old}/subsystem/Webcam.java (92%)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SensorColor.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleop.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java

diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index d2ce72d..c403d3c 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -21,5 +21,10 @@
       <option name="name" value="Google" />
       <option name="url" value="https://dl.google.com/dl/android/maven2/" />
     </remote-repository>
+    <remote-repository>
+      <option name="id" value="maven" />
+      <option name="name" value="maven" />
+      <option name="url" value="https://maven.brott.dev/" />
+    </remote-repository>
   </component>
 </project>
\ No newline at end of file
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 2b0a62f..45085d2 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -26,6 +26,9 @@ android {
 dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
+    implementation 'org.apache.commons:commons-math3:3.6.1'
+    implementation 'com.acmerobotics.roadrunner:core:0.5.5'
+    implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
 }
 
 dependencies {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
deleted file mode 100644
index 3779e49..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
+++ /dev/null
@@ -1,153 +0,0 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.RobotOpMode;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionRight;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionRight.ParkingPosition;
-import org.firstinspires.ftc.teamcode.subsystem.LinearSlide;
-import org.firstinspires.ftc.teamcode.subsystem.StrafeDrive;
-import org.firstinspires.ftc.teamcode.subsystem.Webcam;
-
-@Autonomous(name = "Autonomous Left")
-public class AutoLeft extends RobotOpMode {
-    private final StrafeDrive drive = new StrafeDrive(this);
-    private final LinearSlide slide = new LinearSlide(this);
-    private final SleeveDetectionRight sleeve = new SleeveDetectionRight();
-    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
-
-    @Override
-    public void runOpMode() {
-        addSubsystems(drive, slide, cam);
-
-        // Put motors in encoder mode
-        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        slide.release();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-        ParkingPosition position = sleeve.getPosition();
-        //Place First Cone
-        slide.grab();
-        drive.driveInches(5.5, 5.5, 0.2f);
-        slide.setSlideAndWait(800);
-        drive.strafeInches(38.5, 0.5f);
-        slide.setSlideAndWait(1900);
-        drive.driveInches(6.5, 6.5, 0.2f);
-        slide.release();
-
-        //Go for Second Cone
-        drive.strafeInches(13.0, 0.5f);
-        slide.setSlideAndWait(500);
-        drive.driveInches(20, 20, 0.5f);
-        slide.grab();
-        slide.setSlideAndWait(1000);
-        drive.driveInches(-1, -1, 0.25f);
-        slide.setSlideAndWait(1500);
-        drive.driveInches(-5, -5, 0.25f);
-        slide.setSlideAndWait(300);
-        drive.driveInches(-44, -44, 0.5f);
-        drive.strafeInches(-12.5, 0.5f);
-        slide.setSlideAndWait(3200);
-        drive.driveInches(4, 4, 0.2f);
-        slide.release();
-        drive.driveInches(-4, -4, 0.2f);
-        slide.setSlideAndWait(280);
-        drive.driveInches(0, 1.5, 0.5f);
-        drive.strafeInches(12, 0.5f);
-
-        //Third Cone
-//        drive.driveInches(50, 50, 0.3f);
-//        slide.grab();
-//        slide.setSlide(150, true);
-//        drive.driveInches(-1, -1, 0.25f);
-//        slide.setSlide(375, true);
-//        drive.driveInches(-5, -5, 0.25f);
-//        slide.setSlide(50, true);
-//        drive.driveInches(-38, -38, 0.5f);
-//        slide.setSlide(1000, true);
-//        drive.drive(700, -700, 0.2f);
-//        drive.driveInches(4, 4, 0.2f);
-//        slide.ungrab();
-//        drive.driveInches(-4, -4, 0.2f);
-//        drive.drive(-700, 700, 0.2f);
-
-        //Move to Parking
-        switch (position) {
-            case CENTER:
-                drive.driveInches(24, 24, 0.2f);
-                break;
-            case RIGHT:
-                break;
-            case LEFT:
-                drive.driveInches(48, 48, 0.2f);
-                break;
-        }
-        slide.setSlideAndWait(0);
-        /*
-        1350 is Approximately 25 inches moving forwards
-        1350 is 24 inches moving sideways
-        270 rotation is approximately 35 degrees
-        250 linear slide is 1.5 inches
-
-        Directions
-        strafe 39.5 inches
-        place cone
-        strafe 12 inches
-        rotate 180 degrees
-        move forwards 27 inches
-        */
-
-        //        robot.linearSlide.grab();
-//        drive.driveInches(3, 3, 0.25f);
-//        drive.strafeInches(39.5, 0.25f);
-
-//        drive.drive(200, 200, 0.25f);
-
-        //place first cone (low)
-
-//        robot.linearSlide.unGrab();
-
-//        drive.drive(-200, -200, 0.25f);zs
-//        slide.setSlide(0);
-//        drive.strafeInches(20, 0.25f);
-//        robot.strafeDrive.drive(-926, 926, 0.5f);
-//        drive.driveInches(27, 27, 0.25f);
-        //grab second cone
-//        slide.setSlide(300);
-
-//        robot.linearSlide.grab();
-
-//        slide.setSlide(450);
-//        drive.driveInches(-3, -3, 0.25f);
-//        slide.setSlide(50);
-
-//        drive.driveInches(-37, -37, 0.25f);
-//        drive.strafeInches(-10, 0.5f);
-//        drive.drive(200, 200, 0.5f);
-        //place second cone (mid)
-
-//        slide.setSlide(300);
-//        drive.drive(-200, -200, 0.5f);
-//        drive.strafeInches(10, 0.5f);
-//        drive.driveInches(40, 40, 0.5f);
-
-        //grab third cone
-
-//        drive.driveInches(-35, -35, 0.5f);
-//        drive.drive(-463, 463, 0.5f);
-
-//        place third cone
-
-//        drive.drive(463, -463, 0.5f);
-        //park
-
-//        robot.strafeDrive.drive(1680, 1680, 0.5f);
-//        robot.strafeDrive.strafe(1000, 0.5f);
-//        robot.strafeDrive.strafe(-1000, 0.5f);
-//        robot.strafeDrive.drive(1000, -1000, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, -0.5f);
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
deleted file mode 100644
index 9c571e2..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
+++ /dev/null
@@ -1,199 +0,0 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.RobotOpMode;
-import org.firstinspires.ftc.teamcode.pipeline.PoleDetection;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew.ParkingPosition;
-import org.firstinspires.ftc.teamcode.subsystem.LinearSlide;
-import org.firstinspires.ftc.teamcode.subsystem.StrafeDrive;
-import org.firstinspires.ftc.teamcode.subsystem.Webcam;
-
-@Autonomous(name = "Autonomous Test Left")
-public class AutoTest extends RobotOpMode {
-    private final StrafeDrive drive = new StrafeDrive(this);
-    private final LinearSlide slide = new LinearSlide(this);
-    private final SleeveDetectionNew sleeve = new SleeveDetectionNew();
-    private final PoleDetection pole = new PoleDetection();
-    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
-
-    @Override
-    public void runOpMode() {
-        addSubsystems(drive, slide, cam);
-
-        // Put motors in encoder mode
-        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        slide.release();
-
-        drive.imu.resetYaw();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-        drive.imu.resetYaw();
-        ParkingPosition position = sleeve.getPosition();
-        slide.grab();
-        slide.setSlide(200);
-        drive.driveInches(2, 2, 0.75f);
-//        drive.drive(50,-50, 0.75f);
-        drive.strafeInches(20, 0.6f);
-        drive.driveInches(55, 55, 0.75f);
-//        drive.strafeInches(-12, 0.5f);
-        slide.setSlideAndWait(4000);
-        drive.drive(-375, 375, 0.35f);
-        drive.driveInches(11, 11, 0.2f);
-        drive.driveInches(-4, -4, 0.2f);
-        slide.setSlideAndWait(3250);
-        slide.release();
-
-
-        slide.setSlideAndWait(4000);
-        drive.driveInches(-5, -5, 0.2f);
-        slide.setSlide(600);
-
-//        drive.driveInches(-2250,-2250, 0.5f);
-        //Second CONEEeee
-        drive.drive(-450, 450, 0.35f);
-        drive.strafeInches(-4, 0.6f);
-        drive.driveInches(48, 48, 0.75f);
-//        drive.strafeInches(4, 0.5f);
-//        drive.driveInches(10,10,0.25f);
-        drive.driveInches(-2, -2, 0.5f);
-        slide.grab();
-        slide.setSlideAndWait(1400);
-        drive.driveInches(-5, -5, 0.25f);
-        drive.drive(-50, 50, 0.25f);
-//        drive.strafeInches(-4,0.25f);
-        slide.setSlide(460);
-        drive.driveInches(-16, -16, 0.75f);
-        drive.driveInches(-16, -16, 0.75f);
-        drive.drive(850, -850, 0.25f);
-        slide.setSlideAndWait(4000);
-        drive.driveInches(8, 8, 0.2f);
-        drive.driveInches(-2, -2, 0.2f);
-        slide.setSlideAndWait(3250);
-        slide.release();
-        slide.setSlideAndWait(4000);
-        drive.driveInches(-3, -3, 0.2f);
-        slide.setSlideAndWait(0);
-        //Stuff Work
-
-        //THIRD CONEEEEE
-//        slide.setSlide(300,true);
-//        drive.drive(-725,725,0.35f);
-//        drive.driveInches(21,21,0.5f);
-//        drive.driveInches(16,16,0.5f);
-//        slide.grab();
-//        drive.driveInches(2,2,0.35f);
-//        slide.setSlide(1500,true);
-//        drive.driveInches(-3,-3,0.25f);
-//        slide.setSlide(420,true);
-//        drive.strafeInches(1,0.35f);
-//        drive.driveInches(-32,-32,0.75f);
-//        drive.drive(725,-725,0.25f);
-//        slide.setSlide(4000, true);
-//        drive.driveInches(2,2, 0.2f);
-//        slide.setSlide(3250, true);
-//        slide.ungrab();
-//        drive.driveInches(-2,-2,0.2f);
-//        slide.setSlide(0, true);
-
-        //GAS GAS GAS
-//        drive.driveInches(2,2,1f);
-//        drive.strafeInches(18,0.75f);
-////        drive.turnRight(0, 0.2f);
-//        drive.driveInches(50, 50,0.75f);
-//        drive.strafeInches(-12, 0.5f);
-////        drive.turnRight(0, 0.2f);
-////        cam.camera.setPipeline(pole);
-////        while(!pole.getPole()) {
-////            drive.strafeInches(-2, 0.5f);
-////        }
-//        slide.setSlide(4000, true);
-//        drive.driveInches(4,4, 0.2f);
-//        slide.setSlide(3250, true);
-//        slide.ungrab();
-//        drive.driveInches(-5,-5,0.2f);
-//        slide.setSlide(600, true);
-//
-////        drive.driveInches(-2250,-2250, 0.5f);
-//        //Second CONEEeee
-//        drive.drive(-775,775,0.35f);
-//        drive.driveInches(21,21,1f);
-//        drive.strafeInches(4, 0.5f);
-//        drive.driveInches(16,16,0.5f);
-//        slide.grab();
-//        drive.driveInches(5,5,0.2f);
-//        slide.setSlide(1500,true);
-//        drive.driveInches(-5,-5,0.25f);
-//        slide.setSlide(460,true);
-//        drive.strafeInches(2,0.35f);
-//        drive.driveInches(-16,-16,0.75f);
-//        drive.strafeInches(-2,1f);
-//        drive.driveInches(-16,-16,0.75f);
-//        drive.drive(725,-725,0.25f);
-//        slide.setSlide(4000, true);
-//        drive.driveInches(1,1, 0.2f);
-//        slide.setSlide(3250, true);
-//        slide.ungrab();
-//        drive.driveInches(-3,-3,0.2f);
-//        //Stuff Work
-//
-//        //THIRD CONEEEEE
-////        slide.setSlide(300,true);
-////        drive.drive(-725,725,0.35f);
-////        drive.driveInches(21,21,0.5f);
-////        drive.driveInches(16,16,0.5f);
-////        slide.grab();
-////        drive.driveInches(2,2,0.35f);
-////        slide.setSlide(1500,true);
-////        drive.driveInches(-3,-3,0.25f);
-////        slide.setSlide(420,true);
-////        drive.strafeInches(1,0.35f);
-////        drive.driveInches(-32,-32,0.75f);
-////        drive.drive(725,-725,0.25f);
-////        slide.setSlide(4000, true);
-////        drive.driveInches(2,2, 0.2f);
-////        slide.setSlide(3250, true);
-////        slide.ungrab();
-////        drive.driveInches(-2,-2,0.2f);
-////        slide.setSlide(0, true);
-
-        //Stuff No Work Yet
-        switch (position) {
-            case CENTER:
-                drive.strafeInches(-15, 0.2f);
-                break;
-            case RIGHT:
-                drive.strafeInches(14, 0.2f);
-                break;
-            case LEFT:
-                drive.strafeInches(-38, 0.2f);
-                break;
-        }
-
-//        cycle(700);
-    }
-
-    public void cycle(int height) {
-        drive.drive(-850, 850, 0.25f);
-        drive.driveInches(33, 33, 0.45f);
-        slide.setSlideAndWait(height);
-        drive.driveInches(4, 4, 0.25f);
-        slide.grab();
-        drive.driveInches(-1, -1, 0.15f);
-        slide.setSlideAndWait(height + 600);
-//        drive.driveInches(-2, -2,0.25f);
-//        slide.setSlide(900,true);
-        drive.driveInches(-13, -10, 0.25f);
-        slide.setSlideAndWait(300);
-        drive.driveInches(-25, -25, 0.45f);
-        drive.drive(875, -875, 0.25f);
-        slide.setSlideAndWait(4150);
-        drive.driveInches(2, 2, 0.15f);
-        slide.release();
-        drive.driveInches(-2, -2, 0.15f);
-        slide.setSlideAndWait(0);
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
new file mode 100644
index 0000000..7d3913d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -0,0 +1,120 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+/*
+ * Constants shared between multiple drive types.
+ *
+ * Constants generated by LearnRoadRunner.com/drive-constants
+ *
+ * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
+ * fields may also be edited through the dashboard (connect to the robot's WiFi network and
+ * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
+ *
+ * These are not the only parameters; some are located in the localizer classes, drive base classes,
+ * and op modes themselves.
+ */
+@Config
+public class DriveConstants {
+
+    /*
+     * These are motor constants that should be listed online for your motors.
+     */
+    public static final double TICKS_PER_REV = 28;
+    public static final double MAX_RPM = 6000;
+
+    /*
+     * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
+     * Set this flag to false if drive encoders are not present and an alternative localization
+     * method is in use (e.g., tracking wheels).
+     *
+     * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
+     * from DriveVelocityPIDTuner.
+     */
+    public static final boolean RUN_USING_ENCODER = true;
+    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
+      getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+
+    /*
+     * These are physical constants that can be determined from your robot (including the track
+     * width; it will be tune empirically later although a rough estimate is important). Users are
+     * free to chose whichever linear distance unit they would like so long as it is consistently
+     * used. The default values were selected with inches in mind. Road runner uses radians for
+     * angular distances although most angular parameters are wrapped in Math.toRadians() for
+     * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
+     */
+    public static double WHEEL_RADIUS = 1.4763; // in
+    public static double GEAR_RATIO = 20; // output (wheel) speed / input (motor) speed
+    public static double TRACK_WIDTH = 13.75; // in
+
+    /*
+     * These are the feedforward parameters used to model the drive motor behavior. If you are using
+     * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
+     * motor encoders or have elected not to use them for velocity control, these values should be
+     * empirically tuned.
+     */
+    public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
+    public static double kA = 0;
+    public static double kStatic = 0;
+
+    /*
+     * These values are used to generate the trajectories for you robot. To ensure proper operation,
+     * the constraints should never exceed ~80% of the robot's actual capabilities. While Road
+     * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
+     * small and gradually increase them later after everything is working. All distance units are
+     * inches.
+     */
+    /*
+     * Note from LearnRoadRunner.com:
+     * The velocity and acceleration constraints were calculated based on the following equation:
+     * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
+     * Resulting in 15768.972997281679 in/s.
+     * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
+     * This is capped at 85% because there are a number of variables that will prevent your bot from actually
+     * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
+     * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically 
+     * max velocity. The theoretically maximum velocity is 18551.732937978446 in/s.
+     * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
+     * affected if it is aiming for a velocity not actually possible.
+     * 
+     * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
+     * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
+     * to degrade. As of now, it simply mirrors the velocity, resulting in 15768.972997281679 in/s/s
+     *
+     * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
+     * You are free to raise this on your own if you would like. It is best determined through experimentation.
+     *
+     * WARNING: LearnRoadRunner.com's constant generator has capped the calculated recommended velocity at 90in/s.
+     * This message is showing because your gear ratio/motor RPM/etc. configuration, results in a recommended
+     * velocity (85% of max velocity) exceeding 90in/s.
+     * (Your recommended velocity was 15768.972997281679in/s)
+     * This is simply insanely fast for an FTC bot and chances are your bot cannot properly reach these speeds.
+     * 
+     * Just to be safe, LearnRoadRunner.com has arbitrarily limited your velocity to 90in/s.
+     * You are free to increase it yourself. If you do run into issues, please lower the maximum velocity.
+     * 
+     * A documented case of a similar error which served as an impetus for this reasoning can be found here:
+     * https://github.com/acmerobotics/road-runner-quickstart/issues/91
+     
+     */
+    public static double MAX_VEL = 90;
+    public static double MAX_ACCEL = 90;
+    public static double MAX_ANG_VEL = Math.toRadians(360);
+    public static double MAX_ANG_ACCEL = Math.toRadians(360);
+
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    public static double rpmToVelocity(double rpm) {
+        return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
+    }
+
+    public static double getMotorVelocityF(double ticksPerSecond) {
+      // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
+      return 32767 / ticksPerSecond;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
new file mode 100644
index 0000000..e59e116
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -0,0 +1,297 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.MecanumDrive;
+import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+/*
+ * Simple mecanum drive hardware implementation for REV hardware.
+ */
+@Config
+public class SampleMecanumDrive extends MecanumDrive {
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+
+    public static double LATERAL_MULTIPLIER = 1;
+
+    public static double VX_WEIGHT = 1;
+    public static double VY_WEIGHT = 1;
+    public static double OMEGA_WEIGHT = 1;
+
+    private TrajectorySequenceRunner trajectorySequenceRunner;
+
+    private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+    private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
+
+    private TrajectoryFollower follower;
+
+    private DcMotorEx leftFront, leftRear, rightRear, rightFront;
+    private List<DcMotorEx> motors;
+
+    private IMU imu;
+    private VoltageSensor batteryVoltageSensor;
+
+    public SampleMecanumDrive(HardwareMap hardwareMap) {
+        super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
+
+        follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
+                new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+        LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+
+        // TODO: adjust the names of the following hardware devices to match your configuration
+        imu = hardwareMap.get(IMU.class, "imu");
+        // TODO: Adjust the orientations here to match your robot. See the FTC SDK documentation for
+        // details
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
+        imu.initialize(parameters);
+
+        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+        leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
+        rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
+        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+
+        motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        if (RUN_USING_ENCODER) {
+            setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        }
+
+        setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+            setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+        }
+
+        // TODO: reverse any motors using DcMotor.setDirection()
+
+        // TODO: if desired, use setLocalizer() to change the localization method
+        // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
+
+        trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+        return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+        return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+        return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+        return new TrajectorySequenceBuilder(
+                startPose,
+                VEL_CONSTRAINT, ACCEL_CONSTRAINT,
+                MAX_ANG_VEL, MAX_ANG_ACCEL
+        );
+    }
+
+    public void turnAsync(double angle) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(
+                trajectorySequenceBuilder(getPoseEstimate())
+                        .turn(angle)
+                        .build()
+        );
+    }
+
+    public void turn(double angle) {
+        turnAsync(angle);
+        waitForIdle();
+    }
+
+    public void followTrajectoryAsync(Trajectory trajectory) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(
+                trajectorySequenceBuilder(trajectory.start())
+                        .addTrajectory(trajectory)
+                        .build()
+        );
+    }
+
+    public void followTrajectory(Trajectory trajectory) {
+        followTrajectoryAsync(trajectory);
+        waitForIdle();
+    }
+
+    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+    }
+
+    public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+        followTrajectorySequenceAsync(trajectorySequence);
+        waitForIdle();
+    }
+
+    public Pose2d getLastError() {
+        return trajectorySequenceRunner.getLastPoseError();
+    }
+
+    public void update() {
+        updatePoseEstimate();
+        DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+        if (signal != null) setDriveSignal(signal);
+    }
+
+    public void waitForIdle() {
+        while (!Thread.currentThread().isInterrupted() && isBusy())
+            update();
+    }
+
+    public boolean isBusy() {
+        return trajectorySequenceRunner.isBusy();
+    }
+
+    public void setMode(DcMotor.RunMode runMode) {
+        for (DcMotorEx motor : motors) {
+            motor.setMode(runMode);
+        }
+    }
+
+    public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(zeroPowerBehavior);
+        }
+    }
+
+    public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+        PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+                coefficients.p, coefficients.i, coefficients.d,
+                coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+        );
+
+        for (DcMotorEx motor : motors) {
+            motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+        }
+    }
+
+    public void setWeightedDrivePower(Pose2d drivePower) {
+        Pose2d vel = drivePower;
+
+        if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
+                + Math.abs(drivePower.getHeading()) > 1) {
+            // re-normalize the powers according to the weights
+            double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+                    + VY_WEIGHT * Math.abs(drivePower.getY())
+                    + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+            vel = new Pose2d(
+                    VX_WEIGHT * drivePower.getX(),
+                    VY_WEIGHT * drivePower.getY(),
+                    OMEGA_WEIGHT * drivePower.getHeading()
+            ).div(denom);
+        }
+
+        setDrivePower(vel);
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        List<Double> wheelPositions = new ArrayList<>();
+        for (DcMotorEx motor : motors) {
+            wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition()));
+        }
+        return wheelPositions;
+    }
+
+    @Override
+    public List<Double> getWheelVelocities() {
+        List<Double> wheelVelocities = new ArrayList<>();
+        for (DcMotorEx motor : motors) {
+            wheelVelocities.add(encoderTicksToInches(motor.getVelocity()));
+        }
+        return wheelVelocities;
+    }
+
+    @Override
+    public void setMotorPowers(double v, double v1, double v2, double v3) {
+        leftFront.setPower(v);
+        leftRear.setPower(v1);
+        rightRear.setPower(v2);
+        rightFront.setPower(v3);
+    }
+
+    @Override
+    public double getRawExternalHeading() {
+        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    @Override
+    public Double getExternalHeadingVelocity() {
+        return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
+    }
+
+    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+        return new MinVelocityConstraint(Arrays.asList(
+                new AngularVelocityConstraint(maxAngularVel),
+                new MecanumVelocityConstraint(maxVel, trackWidth)
+        ));
+    }
+
+    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+        return new ProfileAccelerationConstraint(maxAccel);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
new file mode 100644
index 0000000..cbfef3b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
@@ -0,0 +1,304 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.TankDrive;
+import com.acmerobotics.roadrunner.followers.TankPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
+
+import java.util.Arrays;
+import java.util.List;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+/*
+ * Simple tank drive hardware implementation for REV hardware.
+ */
+@Config
+public class SampleTankDrive extends TankDrive {
+    public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0);
+    public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+
+    public static double VX_WEIGHT = 1;
+    public static double OMEGA_WEIGHT = 1;
+
+    private TrajectorySequenceRunner trajectorySequenceRunner;
+
+    private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+    private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL);
+
+    private TrajectoryFollower follower;
+
+    private List<DcMotorEx> motors, leftMotors, rightMotors;
+    private IMU imu;
+
+    private VoltageSensor batteryVoltageSensor;
+
+    public SampleTankDrive(HardwareMap hardwareMap) {
+        super(kV, kA, kStatic, TRACK_WIDTH);
+
+        follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID,
+                new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+        LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+
+        // TODO: adjust the names of the following hardware devices to match your configuration
+        imu = hardwareMap.get(IMU.class, "imu");
+        // TODO: Adjust the orientations here to match your robot. See the FTC SDK documentation for
+        // details
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
+        imu.initialize(parameters);
+
+        // add/remove motors depending on your robot (e.g., 6WD)
+        DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+        DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
+        DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
+        DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+
+        motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+        leftMotors = Arrays.asList(leftFront, leftRear);
+        rightMotors = Arrays.asList(rightFront, rightRear);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        if (RUN_USING_ENCODER) {
+            setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        }
+
+        setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+            setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+        }
+
+        // TODO: reverse any motors using DcMotor.setDirection()
+
+        // TODO: if desired, use setLocalizer() to change the localization method
+        // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
+
+        trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+        return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+        return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+        return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint);
+    }
+
+    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+        return new TrajectorySequenceBuilder(
+                startPose,
+                VEL_CONSTRAINT, accelConstraint,
+                MAX_ANG_VEL, MAX_ANG_ACCEL
+        );
+    }
+
+    public void turnAsync(double angle) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(
+                trajectorySequenceBuilder(getPoseEstimate())
+                        .turn(angle)
+                        .build()
+        );
+    }
+
+    public void turn(double angle) {
+        turnAsync(angle);
+        waitForIdle();
+    }
+
+    public void followTrajectoryAsync(Trajectory trajectory) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(
+                trajectorySequenceBuilder(trajectory.start())
+                        .addTrajectory(trajectory)
+                        .build()
+        );
+    }
+
+    public void followTrajectory(Trajectory trajectory) {
+        followTrajectoryAsync(trajectory);
+        waitForIdle();
+    }
+
+    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+    }
+
+    public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+        followTrajectorySequenceAsync(trajectorySequence);
+        waitForIdle();
+    }
+
+    public Pose2d getLastError() {
+        return trajectorySequenceRunner.getLastPoseError();
+    }
+
+
+    public void update() {
+        updatePoseEstimate();
+        DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+        if (signal != null) setDriveSignal(signal);
+    }
+
+    public void waitForIdle() {
+        while (!Thread.currentThread().isInterrupted() && isBusy())
+            update();
+    }
+
+    public boolean isBusy() {
+        return trajectorySequenceRunner.isBusy();
+    }
+
+    public void setMode(DcMotor.RunMode runMode) {
+        for (DcMotorEx motor : motors) {
+            motor.setMode(runMode);
+        }
+    }
+
+    public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(zeroPowerBehavior);
+        }
+    }
+
+    public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+        PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+                coefficients.p, coefficients.i, coefficients.d,
+                coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+        );
+        for (DcMotorEx motor : motors) {
+            motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+        }
+    }
+
+    public void setWeightedDrivePower(Pose2d drivePower) {
+        Pose2d vel = drivePower;
+
+        if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) {
+            // re-normalize the powers according to the weights
+            double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+                    + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+            vel = new Pose2d(
+                    VX_WEIGHT * drivePower.getX(),
+                    0,
+                    OMEGA_WEIGHT * drivePower.getHeading()
+            ).div(denom);
+        } else {
+            // Ensure the y axis is zeroed out.
+            vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading());
+        }
+
+        setDrivePower(vel);
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        double leftSum = 0, rightSum = 0;
+        for (DcMotorEx leftMotor : leftMotors) {
+            leftSum += encoderTicksToInches(leftMotor.getCurrentPosition());
+        }
+        for (DcMotorEx rightMotor : rightMotors) {
+            rightSum += encoderTicksToInches(rightMotor.getCurrentPosition());
+        }
+        return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+    }
+
+    public List<Double> getWheelVelocities() {
+        double leftSum = 0, rightSum = 0;
+        for (DcMotorEx leftMotor : leftMotors) {
+            leftSum += encoderTicksToInches(leftMotor.getVelocity());
+        }
+        for (DcMotorEx rightMotor : rightMotors) {
+            rightSum += encoderTicksToInches(rightMotor.getVelocity());
+        }
+        return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+    }
+
+    @Override
+    public void setMotorPowers(double v, double v1) {
+        for (DcMotorEx leftMotor : leftMotors) {
+            leftMotor.setPower(v);
+        }
+        for (DcMotorEx rightMotor : rightMotors) {
+            rightMotor.setPower(v1);
+        }
+    }
+
+    @Override
+    public double getRawExternalHeading() {
+        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    @Override
+    public Double getExternalHeadingVelocity() {
+        return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
+    }
+
+    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+        return new MinVelocityConstraint(Arrays.asList(
+                new AngularVelocityConstraint(maxAngularVel),
+                new TankVelocityConstraint(maxVel, trackWidth)
+        ));
+    }
+
+    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+        return new ProfileAccelerationConstraint(maxAccel);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
new file mode 100644
index 0000000..7d039db
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.teamcode.util.Encoder;
+
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |
+ *    | ||        || |
+ *    | ||        || |
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *
+ */
+@Config
+public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
+    public static double TICKS_PER_REV = 0;
+    public static double WHEEL_RADIUS = 2; // in
+    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+
+    public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels
+    public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel
+
+    private Encoder leftEncoder, rightEncoder, frontEncoder;
+
+    public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
+        super(Arrays.asList(
+                new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
+                new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
+                new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
+        ));
+
+        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder"));
+        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder"));
+        frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder"));
+
+        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+    }
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        return Arrays.asList(
+                encoderTicksToInches(leftEncoder.getCurrentPosition()),
+                encoderTicksToInches(rightEncoder.getCurrentPosition()),
+                encoderTicksToInches(frontEncoder.getCurrentPosition())
+        );
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelVelocities() {
+        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
+        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
+        //  compensation method
+
+        return Arrays.asList(
+                encoderTicksToInches(leftEncoder.getRawVelocity()),
+                encoderTicksToInches(rightEncoder.getRawVelocity()),
+                encoderTicksToInches(frontEncoder.getRawVelocity())
+        );
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BOEcode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BOEcode.java
deleted file mode 100644
index dc18033..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BOEcode.java
+++ /dev/null
@@ -1,114 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.BoeDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class BOEcode extends LinearOpMode {
-
-    BoeDetection coneDetection = new BoeDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        coneDetection = new BoeDetection();
-        camera.setPipeline(coneDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
-            @Override
-            public void onOpened() {
-                camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {
-            }
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("Cone: ", coneDetection.getPosition());
-            telemetry.update();
-        }
-
-
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(-520, 520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(375, -375, 0.5f);
-        int straight = 0;
-        boolean turningRight = true;
-        while (coneDetection.getPosition() == BoeDetection.Cone.UNDECIDED || coneDetection.getPosition() == BoeDetection.Cone.RED) {
-
-            if (turningRight) {
-                robot.backTankDrive.drive(-10, 10, 0.5f);
-                straight += 10;
-            } else {
-                robot.backTankDrive.drive(10, -10, 0.5f);
-                straight -= 10;
-            }
-            if (straight >= 500) {
-                turningRight = false;
-            } else if (straight <= -500) {
-                turningRight = true;
-            }
-        }
-
-        robot.backTankDrive.backLeftMotor.setPower(0);
-        robot.backTankDrive.backRightMotor.setPower(0);
-
-        switch (coneDetection.getPosition()) {
-            case RED:
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(-3120, -3120, 0.5f);
-                robot.singleJointGripperArm.ungrab();
-                robot.backTankDrive.drive(straight, -straight, 0.5f);
-                robot.backTankDrive.drive(-1000, 1000, 0.25f);
-                robot.backTankDrive.drive(-3000, -3000, 0.5f);
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(1000, 1000, 0.5f);
-                break;
-            case BLUE:
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(-3120, -3120, 0.5f);
-                robot.singleJointGripperArm.ungrab();
-                robot.backTankDrive.drive(straight, -straight, 0.5f);
-                robot.backTankDrive.drive(1000, -1000, 0.25f);
-                robot.backTankDrive.drive(-3000, -3000, 0.5f);
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(1000, 1000, 0.5f);
-                break;
-
-            default:
-
-                break;
-        }
-
-    }
-
-
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BetterRobotAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BetterRobotAutonomous.java
deleted file mode 100644
index 310d4d3..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/BetterRobotAutonomous.java
+++ /dev/null
@@ -1,91 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.old.Util.Maths;
-
-@Disabled
-public class BetterRobotAutonomous extends LinearOpMode {
-    private DcMotor left;
-    private DcMotor right;
-    private DcMotor arm;
-    private Servo grip1;
-    private Servo grip2;
-    private int lPos;
-    private int rPos;
-    private double gripMin = 0.45;
-    private double gripMax = 1.0;
-
-    @Override
-    public void runOpMode() {
-        left = hardwareMap.get(DcMotor.class, "b_left");
-        right = hardwareMap.get(DcMotor.class, "b_right");
-        arm = hardwareMap.get(DcMotor.class, "arm");
-        grip1 = hardwareMap.get(Servo.class, "grip1");
-        grip2 = hardwareMap.get(Servo.class, "grip2");
-
-        //Put motors in encoder mode
-        left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-
-        right.setDirection(DcMotorSimple.Direction.REVERSE);
-        //You need to have wait for start or else bad things happen
-        ungrab();
-        waitForStart();
-
-        grab();
-        move(2080, 2080, 0.50);
-        armSet(89);
-        move(520, -520, 0.50);
-        ungrab();
-        move(-520, 520, 0.50);
-        armSet(0);
-        move(2080, 2080, 0.50);
-        move(-520, 520, 0.50);
-        move(520, -520, 0.50);
-
-
-
-
-    }
-    private void move(int leftTarget, int rightTarget, double speed) {
-        lPos += leftTarget;
-        rPos += rightTarget;
-
-        left.setTargetPosition(lPos);
-        right.setTargetPosition(rPos);
-
-        left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
-        left.setPower(speed);
-        right.setPower(speed);
-
-        while(opModeIsActive() && left.isBusy() && right.isBusy()) {
-            idle();
-        }
-    }
-
-    private void armSet(int steps) {
-        arm.setTargetPosition(Maths.clamp(steps, 0, 450));
-        while (opModeIsActive() && arm.isBusy()) {
-            idle();
-        }
-    }
-    public void grab() {
-        grip1.setPosition(gripMax);
-        grip2.setPosition(1-gripMax);
-    }
-
-    public void ungrab() {
-        grip1.setPosition(gripMin);
-        grip2.setPosition(1-gripMin);
-    }
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoLeft.java
deleted file mode 100644
index ab176eb..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoLeft.java
+++ /dev/null
@@ -1,137 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.SleeveDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class NewBotAutoLeft extends LinearOpMode {
-
-    SleeveDetection sleeveDetection = new SleeveDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        sleeveDetection = new SleeveDetection();
-        camera.setPipeline(sleeveDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
-        {
-            @Override
-            public void onOpened()
-            {
-                camera.startStreaming(320,240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {}
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("ROTATION: ", sleeveDetection.getPosition());
-            telemetry.update();
-        }
-
-
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(-520, 520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(375, -375, 0.5f);
-
-
-        switch(sleeveDetection.getPosition()) {
-
-            case LEFT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(3000, 3000, 0.5f);
-//                robot.backTankDrive.drive(850, -850, 0.5f);
-//                robot.singleJointGripperArm.setArm(89);
-//                robot.singleJointGripperArm.ungrab();
-//                robot.backTankDrive.drive(-3300, -3300, 0.5f);
-//                robot.backTankDrive.drive(-450, 450, 0.25f);
-                robot.backTankDrive.drive(-2200, -2200, 0.75f);
-                break;
-            case RIGHT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(150, 150, 0.5f);
-//                robot.backTankDrive.drive(-900, 900, 0.5f);
-//                robot.backTankDrive.drive(-2300, -2300, 0.5f);
-//                robot.backTankDrive.drive(900, -900, 0.5f);
-                robot.backTankDrive.drive(2450, 2450, 0.5f);
-                break;
-            default:
-                placeMiddle(robot);
-                break;
-        }
-
-    }
-    public void placeMiddle(OldRobot robot){
-        //First Cone
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(-3120, -3120, 1f);
-        robot.singleJointGripperArm.setArm(450);
-        robot.backTankDrive.drive(-500, 500, 0.6f);
-        robot.backTankDrive.drive(-350, -350, 0.25f);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(500, 500, 0.25f);
-        robot.backTankDrive.drive(500, -500, 0.6f);
-        robot.singleJointGripperArm.setArm(0);
-
-        //Second Cone
-        robot.backTankDrive.drive(-2500, -2500, 1f);
-        robot.backTankDrive.drive(450, 450, 1f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.backTankDrive.drive(1100, -1100, 0.5f);
-        robot.backTankDrive.drive(-1175, -1175, 0.5f);
-
-        robot.singleJointGripperArm.ungrab();
-        robot.singleJointGripperArm.setArm(300);
-        robot.backTankDrive.drive(200, -200, 0.6f);
-        robot.backTankDrive.drive(2000,2000, 0.5f);
-        robot.backTankDrive.drive(375, -375, 0.5f);
-        robot.backTankDrive.drive(-200,-200, 0.5f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(200,200, 0.5f);
-        robot.backTankDrive.drive(-375, 375, 0.5f);
-        robot.singleJointGripperArm.setArm(100);
-
-        //Third Cone
-        robot.backTankDrive.drive(-2000,-2000, 0.5f);
-        robot.singleJointGripperArm.ungrab();
-        robot.singleJointGripperArm.setArm(250);
-        robot.backTankDrive.drive(-1175, -1175, 0.5f);
-        robot.backTankDrive.drive(-375, 375, 0.5f);
-        robot.singleJointGripperArm.setArm(0);
-        robot.singleJointGripperArm.ungrab();
-        robot.singleJointGripperArm.setArm(100);
-        robot.backTankDrive.drive(375, -375, 0.5f);
-    }
-
-
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoRight.java
deleted file mode 100644
index b4e99d8..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/NewBotAutoRight.java
+++ /dev/null
@@ -1,147 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.SleeveDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class NewBotAutoRight extends LinearOpMode {
-
-    SleeveDetection sleeveDetection = new SleeveDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        sleeveDetection = new SleeveDetection();
-        camera.setPipeline(sleeveDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
-        {
-            @Override
-            public void onOpened()
-            {
-                camera.startStreaming(320,240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {}
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("ROTATION: ", sleeveDetection.getPosition());
-            telemetry.update();
-        }
-
-
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(-520, 520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(375, -375, 0.5f);
-        switch(sleeveDetection.getPosition()) {
-            case LEFT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(150, 150, 0.5f);
-//                robot.backTankDrive.drive(900, -900, 0.5f);
-//                robot.backTankDrive.drive(-2300, -2300, \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\0.5f);
-//                robot.backTankDrive.drive(-900, 900, 0.5f);
-                robot.backTankDrive.drive(2450, 2450, 0.5f);
-
-                break;
-            case RIGHT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(3000, 3000, 0.5f);
-//                robot.backTankDrive.drive(-850, 850, 0.5f);
-//                robot.singleJointGripperArm.setArm(89);
-//                robot.singleJointGripperArm.ungrab();
-//                robot.backTankDrive.drive(-3300, -3300, 0.5f);
-//                robot.backTankDrive.drive(450, -450, 0.25f);
-                robot.backTankDrive.drive(-2200, -2200, 0.75f);
-                break;
-            default:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(1000, 1000, 0.5f);
-                break;
-        }
-
-    }
-
-//    public void placeMiddle2(OldRobot robot){
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-3120, -3120, 0.5f);
-//        robot.singleJointGripperArm.setArm(450);
-//        robot.backTankDrive.drive(500, -500, 0.5f);
-//        robot.backTankDrive.drive(-450, -450, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(-375, 375, 0.5f);
-//    }
-
-    public void placeMiddle(OldRobot robot){
-        //First Cone
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(-3120, -3120, 1f);
-        robot.singleJointGripperArm.setArm(450);
-        robot.backTankDrive.drive(500, -500, 0.6f);
-        robot.backTankDrive.drive(-350, -350, 0.25f);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(500, 500, 0.25f);
-        robot.backTankDrive.drive(-500, 500, 0.6f);
-        robot.singleJointGripperArm.setArm(0);
-
-        //Second Cone
-        robot.backTankDrive.drive(-2300, -2300, 1f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.backTankDrive.drive(-1100, 1100, 0.5f);
-        robot.backTankDrive.drive(-1150, -1150, 0.5f);
-
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(50, 50, 0.5f);
-        robot.singleJointGripperArm.setArm(300);
-        robot.backTankDrive.drive(-200, 200, 0.6f);
-        robot.backTankDrive.drive(2000,2000, 1f);
-        robot.backTankDrive.drive(-350, 350, 0.5f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(400, -400, 0.5f);
-        robot.singleJointGripperArm.setArm(0);
-
-//        robot.backTankDrive.drive(-1900,-1900, 1f);
-//        robot.backTankDrive.drive(50, 50, 1f);
-//        robot.singleJointGripperArm.ungrab();
-//
-//        robot.singleJointGripperArm.setArm(250);
-//        robot.backTankDrive.drive(1175, 1175, 1f);
-//        robot.backTankDrive.drive(500, -500, 0.5f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.singleJointGripperArm.ungrab();
-//        robot.singleJointGripperArm.setArm(100);
-//        robot.backTankDrive.drive(-500, 500, 0.5f);
-    }
-
-
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomous.java
deleted file mode 100644
index 06eefa0..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomous.java
+++ /dev/null
@@ -1,49 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-
-@Disabled
-public class OldRobotAutonomous extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-            //2080 ticks is 2 rotations
-        //Low Height Pole
-            //robot.singleJointGripperArm.ungrab();
-            //robot.backTankDrive.drive(-400, -400, 0.5f);
-            //robot.backTankDrive.drive(-520, 520, 0.5f);
-            //robot.singleJointGripperArm.setArm(290);
-            //robot.backTankDrive.drive(-200, -200, 0.25f);
-            //robot.singleJointGripperArm.grab();
-            //robot.backTankDrive.drive(520, 520, 0.25f);
-            //robot.singleJointGripperArm.setArm(0);
-            //robot.backTankDrive.drive(375, -375, 0.5f);
-            //robot.backTankDrive.drive(-3120, -3120, 0.5f);
-            //IMPORTANT: TO GO FORWARD PUT NEGATIVE VALUES
-        //Mid Height Pole
-            robot.singleJointGripperArm.ungrab();
-            robot.backTankDrive.drive(-3120, -3120, 0.5f);
-            robot.singleJointGripperArm.setArm(450);
-            robot.backTankDrive.drive(-520, 520, 0.5f);
-            robot.backTankDrive.drive(-400, -400, 0.25f);
-            robot.singleJointGripperArm.grab();
-            robot.backTankDrive.drive(520, 520, 0.25f);
-            robot.singleJointGripperArm.setArm(0);
-            robot.backTankDrive.drive(375, -375, 0.5f);
-            robot.backTankDrive.drive(-400, -400, 0.5f);
-    }
-
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomousOther.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomousOther.java
deleted file mode 100644
index 96d78f0..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/OldRobotAutonomousOther.java
+++ /dev/null
@@ -1,50 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-
-@Disabled
-public class OldRobotAutonomousOther extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-        //2080 ticks is 2 rotations
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(520, -520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(-375, 375, 0.5f);
-//        robot.backTankDrive.drive(-3120, -3120, 0.5f);
-//          IMPORTANT: TO GO FORWARD PUT NEGATIVE VALUES
-
-//Mid Height Pole
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(-3120, -3120, 0.5f);
-        robot.singleJointGripperArm.setArm(450);
-        robot.backTankDrive.drive(520, -520, 0.5f);
-        robot.backTankDrive.drive(-400, -400, 0.25f);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(520, 520, 0.25f);
-        robot.singleJointGripperArm.setArm(0);
-        robot.backTankDrive.drive(-375, 375, 0.5f);
-        robot.backTankDrive.drive(-400, -400, 0.5f);
-    }
-
-
-}
-
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/StrafeBotAutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/StrafeBotAutoLeft.java
deleted file mode 100644
index b09a229..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Auto/StrafeBotAutoLeft.java
+++ /dev/null
@@ -1,55 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.NewRobot;
-
-@Disabled
-public class StrafeBotAutoLeft extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        NewRobot robot = new NewRobot(this, hardwareMap, telemetry, gamepad1);
-        //Put motors in encoder mode
-        robot.strafeDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.strafeDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.strafeDrive.frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.strafeDrive.frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-//        robot.linearSlide.slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-        //1350 is Approximately 25 inches moving forwards
-        //1350 is 24 inches moving sideways
-        //270 rotation is approximately 35 degrees
-
-        //Directions
-        //strafe 39.5 inches
-        //place cone
-        //strafe 12 inches
-        //rotate 180 degrees
-        //move forwards 27 inches
-
-//        robot.strafeDrive.strafe(-2221, 0.5f);
-//        robot.strafeDrive.drive(162, 162, 0.5f);
-//        robot.strafeDrive.drive(-162, -162, 0.5f);
-//        robot.strafeDrive.strafe(675, 0.5f);
-//        robot.linearSlide.grab();
-        robot.strafeDrive.strafein(-39.5, 0.5f);
-        robot.strafeDrive.strafe(200, 0.5f);
-//        robot.linearSlide.setSlide(100);
-//        robot.linearSlide.ungrab();
-//        robot.linearSlide.setSlide(0);
-        robot.strafeDrive.strafein(-12, 0.5f);
-        robot.strafeDrive.drive(-926, 926, 0.5f);
-        robot.strafeDrive.strafein(-27, 0.5f);
-
-//        robot.strafeDrive.drive(1680, 1680, 0.5f);
-//        robot.strafeDrive.strafe(1000, 0.5f);
-//        robot.strafeDrive.strafe(-1000, 0.5f);
-//        robot.strafeDrive.drive(1000, -1000, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, -0.5f);
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
similarity index 84%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotOpMode.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
index 808b904..255ac0c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotOpMode.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
@@ -1,11 +1,9 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.old;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.subsystem.Subsystem;
-
-import java.util.ArrayList;
+import org.firstinspires.ftc.teamcode.old.subsystem.Subsystem;
 
 abstract public class RobotOpMode extends LinearOpMode {
     public void addSubsystems(Subsystem... subsystems) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/NewRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/NewRobot.java
deleted file mode 100644
index 3384481..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/NewRobot.java
+++ /dev/null
@@ -1,84 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Robots;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.Subsystems.StrafeDrive;
-import org.firstinspires.ftc.teamcode.old.Subsystems.LinearSlide;
-
-public class NewRobot {
-    private final HardwareMap hardwareMap;
-    private final Telemetry telemetry;
-
-    // Hardware Configuration Names
-    public static final String FRONT_LEFT_MOTOR_NAME = "motor_front_left";
-    public static final String FRONT_RIGHT_MOTOR_NAME = "motor_front_right";
-    public static final String BACK_LEFT_MOTOR_NAME = "motor_back_left";
-    public static final String BACK_RIGHT_MOTOR_NAME = "motor_back_right";
-    public static final String LINEAR_SLIDE_MOTOR_NAME = "motor_slide";
-    public static final String GRIPPER1_SERVO_NAME = "grip1";
-    public static final String GRIPPER2_SERVO_NAME = "grip2";
-
-    // Subsystems
-    public final StrafeDrive strafeDrive;
-    public final LinearOpMode linearOpMode;
-    public final LinearSlide linearSlide;
-
-    // Sensors
-    public final Gamepad gamepad;
-
-    public NewRobot(LinearOpMode linearOpMode, HardwareMap hardwareMap, Telemetry telemetry, Gamepad gamepad) {
-        this.hardwareMap = hardwareMap;
-        this.telemetry = telemetry;
-        this.linearOpMode = linearOpMode;
-        this.gamepad = gamepad;
-        this.strafeDrive = initStrafeDrive();
-        this.linearSlide = initLinearSlide();
-    }
-    
-    public StrafeDrive initStrafeDrive() {
-        DcMotor frontLeftMotor = (DcMotor) hardwareMap.get(FRONT_LEFT_MOTOR_NAME);
-        DcMotor frontRightMotor = (DcMotor) hardwareMap.get(FRONT_RIGHT_MOTOR_NAME);
-        DcMotor backLeftMotor = (DcMotor) hardwareMap.get(BACK_LEFT_MOTOR_NAME);
-        DcMotor backRightMotor = (DcMotor) hardwareMap.get(BACK_RIGHT_MOTOR_NAME);
-
-        backLeftMotor.setDirection(DcMotor.Direction.REVERSE);
-        frontLeftMotor.setDirection(DcMotor.Direction.REVERSE);
-
-        return new StrafeDrive(linearOpMode,
-                telemetry,
-                frontLeftMotor,
-                frontRightMotor,
-                backLeftMotor,
-                backRightMotor,
-                gamepad
-        );
-    }
-
-    public LinearSlide initLinearSlide(){
-        DcMotor slideMotor = (DcMotor) hardwareMap.get(LINEAR_SLIDE_MOTOR_NAME);
-        slideMotor.setTargetPosition(0);
-        slideMotor.setPower(1);
-        slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        slideMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
-        Servo gripper1Servo = (Servo) hardwareMap.get(GRIPPER1_SERVO_NAME);
-        Servo gripper2Servo = (Servo) hardwareMap.get(GRIPPER2_SERVO_NAME);
-        return new LinearSlide(
-                linearOpMode,
-                telemetry,
-                slideMotor,
-                gripper1Servo,
-                gripper2Servo,
-                gamepad,
-                450,
-                0,
-                0.45,
-                1
-                );
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/OldRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/OldRobot.java
deleted file mode 100644
index 441d548..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Robots/OldRobot.java
+++ /dev/null
@@ -1,83 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Robots;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.Subsystems.BackTankDrive;
-import org.firstinspires.ftc.teamcode.old.Subsystems.SingleJointGripperArm;
-
-public class OldRobot {
-    private final HardwareMap hardwareMap;
-    private final Telemetry telemetry;
-
-    // Hardware Configuration Names
-    public static final String BACK_LEFT_MOTOR_NAME = "b_left";
-    public static final String BACK_RIGHT_MOTOR_NAME = "b_right";
-    public static final String ARM_MOTOR_NAME = "arm";
-    public static final String GRIPPER1_SERVO_NAME = "grip1";
-    public static final String GRIPPER2_SERVO_NAME = "grip2";
-
-    // Subsystems
-    public final BackTankDrive backTankDrive;
-    public final SingleJointGripperArm singleJointGripperArm;
-    public final LinearOpMode linearOpMode;
-
-    // Sensors
-    public final Gamepad gamepad;
-
-    public OldRobot(LinearOpMode linearOpMode, HardwareMap hardwareMap, Telemetry telemetry, Gamepad gamepad) {
-        this.linearOpMode = linearOpMode;
-        this.hardwareMap = hardwareMap;
-        this.telemetry = telemetry;
-        this.gamepad = gamepad;
-        this.backTankDrive = initBackTankDrive();
-        this.singleJointGripperArm = initSingleJointGripperArm();
-    }
-
-    public BackTankDrive initBackTankDrive() {
-        DcMotor backLeftMotor = (DcMotor) hardwareMap.get(BACK_LEFT_MOTOR_NAME);
-        DcMotor backRightMotor = (DcMotor) hardwareMap.get(BACK_RIGHT_MOTOR_NAME);
-
-        backRightMotor.setDirection(DcMotor.Direction.REVERSE);
-
-        return new BackTankDrive(
-                linearOpMode,
-                telemetry,
-                backLeftMotor,
-                backRightMotor,
-                gamepad,
-                1,
-                5000, // TODO: get what this value is in class
-                0.1 // TODO: measure in class what this value is
-        );
-    }
-
-    public SingleJointGripperArm initSingleJointGripperArm() {
-        DcMotor armMotor = (DcMotor) hardwareMap.get(ARM_MOTOR_NAME);
-
-        armMotor.setTargetPosition(0);
-        armMotor.setPower(1);
-        armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
-        Servo gripper1Servo = (Servo) hardwareMap.get(GRIPPER1_SERVO_NAME);
-        Servo gripper2Servo = (Servo) hardwareMap.get(GRIPPER2_SERVO_NAME);
-
-        return new SingleJointGripperArm(
-                linearOpMode,
-                telemetry,
-                armMotor,
-                gripper1Servo,
-                gripper2Servo,
-                gamepad,
-                0.45,
-                1,
-                0,
-                450
-        );
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BackTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BackTankDrive.java
deleted file mode 100644
index ddd1d7f..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BackTankDrive.java
+++ /dev/null
@@ -1,79 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-public class BackTankDrive {
-    private final Telemetry telemetry;
-    public final DcMotor backLeftMotor;
-    public final DcMotor backRightMotor;
-    public final Gamepad gamepad;
-    public final int turnSensitivity;
-    public final int encoderSteps; // TODO: find out what this number is
-    public final double wheelRadius; // in meters
-    public int leftPos = 0;
-    public int rightPos = 0;
-    private LinearOpMode opMode;
-
-    public BackTankDrive(LinearOpMode opmode, Telemetry telemetry, DcMotor backLeftMotor, DcMotor backRightMotor, Gamepad gamepad, int turnSensitivity, int encoderSteps, double wheelRadius) {
-        this.opMode = opmode;
-        this.telemetry = telemetry;
-        this.backLeftMotor = backLeftMotor;
-        this.backRightMotor = backRightMotor;
-        this.gamepad = gamepad;
-        this.turnSensitivity = turnSensitivity;
-        this.encoderSteps = encoderSteps;
-        this.wheelRadius = wheelRadius;
-    }
-
-    public void respondToGamepad() {
-        if (gamepad.right_stick_x < 0.01 && gamepad.right_stick_x > -0.01 && (gamepad.left_stick_y > 0.01 || gamepad.left_stick_y < -0.01)) {
-            // Left-stick control all, going straight
-            backLeftMotor.setPower(gamepad.left_stick_y / 1.5);
-            backRightMotor.setPower(gamepad.left_stick_y / 1.5);
-        } else if (gamepad.right_stick_x < -0.01 && (gamepad.left_stick_y > 0.05 || gamepad.left_stick_y < -0.01)) {
-            // Turning left while moving
-            backLeftMotor.setPower((gamepad.left_stick_y / turnSensitivity) / 2);
-            backRightMotor.setPower(gamepad.left_stick_y / 2);
-        } else if (gamepad.right_stick_x > 0.01 && (gamepad.left_stick_y > 0.05 || gamepad.left_stick_y < -0.01)) {
-            // Turning right wile moving forward
-            backLeftMotor.setPower(gamepad.left_stick_y / 2);
-            backRightMotor.setPower((gamepad.left_stick_y / turnSensitivity) / 2);
-        } else if (gamepad.right_stick_x < -0.01 && (gamepad.left_stick_y > -0.05 || gamepad.left_stick_y < 0.01)) {
-            // Turning left without going forward
-            backRightMotor.setPower(gamepad.right_stick_x / 2);
-            backLeftMotor.setPower(-(gamepad.right_stick_x / 2));
-        } else if (gamepad.right_stick_x > 0.01 && (gamepad.left_stick_y > -0.05 || gamepad.left_stick_y < 0.01)) {
-            // Turning right without moving forward
-            backLeftMotor.setPower(-(gamepad.right_stick_x / 2));
-            backRightMotor.setPower(gamepad.right_stick_x / 2);
-        } else if (gamepad.right_stick_x == 0 && gamepad.left_stick_y == 0) {
-            backLeftMotor.setPower(0);
-            backRightMotor.setPower(0);
-        } else if (gamepad.right_bumper || gamepad.left_bumper) {
-            //covered in gamepadPlusCamera in CameraRobotTeleOp class
-        } else {
-            backLeftMotor.setPower(0);
-            backRightMotor.setPower(0);
-        }
-    }
-
-
-    public void drive(int leftmove, int rightmove, float speed) {
-        leftPos += leftmove;
-        rightPos += rightmove;
-        backLeftMotor.setTargetPosition(leftPos);
-        backRightMotor.setTargetPosition(rightPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-
-        while (opMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy()) {
-            opMode.idle();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BoeDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BoeDetection.java
deleted file mode 100644
index 3338c61..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/BoeDetection.java
+++ /dev/null
@@ -1,185 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class BoeDetection extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum Cone {
-        RED,
-        BLUE,
-        UNDECIDED
-    }
-
-    public enum Distance {
-        CLOSE,
-        MID,
-        FAR,
-        UNSEEN
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static final int X = 55;
-    private static final int Y = 120;
-    private static final int STEP = 10;
-    private static final int WIDTH = 25;
-    private static final int HEIGHT = 15;
-    private static final Point FAR_TOPLEFT_ANCHOR_POINT = new Point(X, Y);
-    private static final Point CLOSE_TOPLEFT_ANCHOR_POINT = new Point(X - (2 * STEP), Y - (2 * STEP));
-
-    // Width and height for the bounding box
-    public static int FAR_REGION_WIDTH = WIDTH;
-    public static int FAR_REGION_HEIGHT = HEIGHT;
-    public static int MID_REGION_WIDTH = WIDTH + STEP + STEP;
-    public static int MID_REGION_HEIGHT = HEIGHT + STEP + STEP;
-    public static int CLOSE_REGION_WIDTH = WIDTH + STEP + STEP + STEP + STEP;
-    public static int CLOSE_REGION_HEIGHT = HEIGHT + STEP + STEP + STEP + STEP;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_red_bounds = new Scalar(100, 0, 0, 255),
-            upper_red_bounds = new Scalar(255, 150, 150, 255),
-            lower_blue_bounds = new Scalar(0, 0, 100, 255),
-            upper_blue_bounds = new Scalar(150, 150, 255, 255);
-
-    // Color definitions
-    private final Scalar
-            RED = new Scalar(255, 0, 0),
-            BLUE = new Scalar(0, 0, 255),
-            BLACK = new Scalar(0, 0, 0);
-
-    // Percent and mat definitions
-    private double redPercentC, bluePercentC, redPercentM, bluePercentM, redPercentF, bluePercentF;
-    private final Mat redMatC = new Mat();
-    private final Mat blueMatC = new Mat();
-    private Mat redMatF = new Mat();
-    private Mat blueMatF = new Mat();
-    private Mat redMatM = new Mat();
-    private Mat blueMatM = new Mat();
-    private Mat blurredMatClose = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point((2 * STEP), (2 * STEP));
-    Point sleeve_pointB = new Point(
-            (2 * STEP) + FAR_REGION_WIDTH,
-            (2 * STEP) + FAR_REGION_HEIGHT);
-
-    Point sleeve_pointC = new Point(STEP, STEP);
-    Point sleeve_pointD = new Point(
-            STEP + MID_REGION_WIDTH,
-            STEP + MID_REGION_HEIGHT);
-
-    Point sleeve_pointE = new Point(
-            CLOSE_TOPLEFT_ANCHOR_POINT.x,
-            CLOSE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointF = new Point(
-            CLOSE_TOPLEFT_ANCHOR_POINT.x + CLOSE_REGION_WIDTH,
-            CLOSE_TOPLEFT_ANCHOR_POINT.y + CLOSE_REGION_HEIGHT);
-
-    // Running variable storing the parking position
-    private volatile Cone cone = Cone.UNDECIDED;
-    private volatile Distance distance = Distance.UNSEEN;
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMatClose, new Size(5, 5));
-        blurredMatClose = blurredMatClose.submat(new Rect(sleeve_pointE, sleeve_pointF));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMatClose, blurredMatClose, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMatClose, lower_red_bounds, upper_red_bounds, redMatC);
-        Core.inRange(blurredMatClose, lower_blue_bounds, upper_blue_bounds, blueMatC);
-
-        redMatM = redMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
-        blueMatM = blueMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
-
-        redMatF = redMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
-        blueMatF = blueMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-
-        // Gets color specific values
-        redPercentC = Core.countNonZero(redMatC);
-        bluePercentC = Core.countNonZero(blueMatC);
-
-        redPercentM = Core.countNonZero(redMatM);
-        bluePercentM = Core.countNonZero(blueMatM);
-
-        redPercentF = Core.countNonZero(redMatF);
-        bluePercentF = Core.countNonZero(blueMatF);
-
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(redPercentF, bluePercentF);
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == redPercentF) {
-            if (redPercentC >= 150) {
-                cone = Cone.RED;
-                distance = Distance.CLOSE;
-            } else if (redPercentM >= 100) {
-                cone = Cone.RED;
-                distance = Distance.MID;
-            } else if (redPercentF >= 50) {
-                cone = Cone.RED;
-                distance = Distance.FAR;
-            } else {
-                cone = Cone.UNDECIDED;
-                distance = Distance.UNSEEN;
-            }
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    RED,
-                    2
-            );
-        } else if (maxPercent == bluePercentC && bluePercentC >= 50) {
-            cone = Cone.BLUE;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    BLUE,
-                    2
-            );
-        } else {
-            cone = Cone.UNDECIDED;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    BLACK,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        redMatC.release();
-        blueMatC.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public Cone getPosition() {
-        return cone;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/FullDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/FullDetection.java
deleted file mode 100644
index 7c560e8..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/FullDetection.java
+++ /dev/null
@@ -1,170 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class FullDetection extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum ParkingPosition {
-        LEFT,
-        CENTER,
-        RIGHT
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(55, 120);
-    private static Point POLE_TOPLEFT_ANCHOR_POINT = new Point(75, 100);
-
-    // Width and height for the bounding box
-    public static int SLEEVE_REGION_WIDTH = 25;
-    public static int SLEEVE_REGION_HEIGHT = 15;
-    public static int POLE_REGION_WIDTH = 25;
-    public static int POLE_REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds  = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds  = new Scalar(255, 255, 150, 255),
-            lower_cyan_bounds    = new Scalar(0, 120, 120, 255),
-            upper_cyan_bounds    = new Scalar(150, 255, 255, 255),
-            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
-            upper_magenta_bounds = new Scalar(255, 170, 255, 255),
-            lower_pole_bounds = new Scalar(150, 150, 0, 255),
-            upper_pole_bounds = new Scalar(255, 255, 150, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW  = new Scalar(255, 255, 0),
-            CYAN    = new Scalar(0, 255, 255),
-            MAGENTA = new Scalar(255, 0, 255);
-
-    // Percent and mat definitions
-    private double yelPercentS, cyaPercentS, magPercentS, yelPercentP;
-    private Mat yelMatS = new Mat(), cyaMatS = new Mat(), magMatS = new Mat(), yelMatP = new Mat(), blurredMat = new Mat(), sleeveMat = new Mat(), poleMat = new Mat(), kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + SLEEVE_REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + SLEEVE_REGION_HEIGHT);
-    Point pole_pointA = new Point(
-            POLE_TOPLEFT_ANCHOR_POINT.x,
-            POLE_TOPLEFT_ANCHOR_POINT.y);
-    Point pole_pointB = new Point(
-            POLE_TOPLEFT_ANCHOR_POINT.x + POLE_REGION_WIDTH,
-            POLE_TOPLEFT_ANCHOR_POINT.y + POLE_REGION_HEIGHT);
-
-    // Running variable storing the parking position and pole detection
-    private volatile ParkingPosition position = ParkingPosition.LEFT;
-    private volatile boolean isPole = false;
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        sleeveMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-        poleMat = blurredMat.submat(new Rect(pole_pointA, pole_pointB));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(sleeveMat, sleeveMat, Imgproc.MORPH_CLOSE, kernel);
-        Imgproc.morphologyEx(poleMat, poleMat, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(sleeveMat, lower_yellow_bounds, upper_yellow_bounds, yelMatS);
-        Core.inRange(sleeveMat, lower_cyan_bounds, upper_cyan_bounds, cyaMatS);
-        Core.inRange(sleeveMat, lower_magenta_bounds, upper_magenta_bounds, magMatS);
-        Core.inRange(poleMat, lower_pole_bounds, upper_pole_bounds, yelMatP);
-
-        // Gets color specific values
-        yelPercentS = Core.countNonZero(yelMatS);
-        cyaPercentS = Core.countNonZero(cyaMatS);
-        magPercentS = Core.countNonZero(magMatS);
-        yelPercentP = Core.countNonZero(yelMatP);
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(yelPercentS, Math.max(cyaPercentS, magPercentS));
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == yelPercentS) {
-            position = ParkingPosition.LEFT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else if (maxPercent == cyaPercentS) {
-            position = ParkingPosition.CENTER;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        } else if (maxPercent == magPercentS) {
-            position = ParkingPosition.RIGHT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    MAGENTA,
-                    2
-            );
-        }
-        if (yelPercentP >= 60) {
-            isPole = true;
-            Imgproc.rectangle(
-                    input,
-                    pole_pointA,
-                    pole_pointB,
-                    YELLOW,
-                    2
-            );
-        } else {
-            isPole = false;
-            Imgproc.rectangle(
-                    input,
-                    pole_pointA,
-                    pole_pointB,
-                    CYAN,
-                    2
-            );
-        }
-        // Memory cleanup
-        blurredMat.release();
-        yelMatS.release();
-        cyaMatS.release();
-        magMatS.release();
-        yelMatP.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public ParkingPosition getPosition() {
-        return position;
-    }
-    public boolean getPole() {
-        return isPole;
-    }
-    public double getPercent() {
-        return yelPercentP;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/LinearSlide.java
deleted file mode 100644
index 705e63b..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/LinearSlide.java
+++ /dev/null
@@ -1,102 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.Util.Maths;
-
-public class LinearSlide {
-    private final Telemetry telemetry;
-    public final DcMotor slideMotor;
-    public final Servo gripper1Servo;
-    public final Servo gripper2Servo;
-    public final Gamepad gamepad;
-    public final int minHeight;
-    public final int maxHeight;
-    public final double gripMin;
-    public final double gripMax;
-    private double gripPos = 1;
-    private double height = 0;
-    private int armMotorSteps =0;
-    private boolean dpadpressed = false;
-    private LinearOpMode linearOpMode;
-
-    public LinearSlide(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor slideMotor, Servo gripper1, Servo gripper2, Gamepad gamepad, int maxHeight, int minHeight, double gripMin, double gripMax){
-        this.linearOpMode = linearOpMode;
-        this.telemetry = telemetry;
-        this.slideMotor = slideMotor;
-        this.gripper1Servo = gripper1;
-        this.gripper2Servo = gripper2;
-        this.gamepad = gamepad;
-        this.minHeight = minHeight;
-        this.maxHeight = maxHeight;
-        this.gripMin = gripMin;
-        this.gripMax = gripMax;
-    }
-
-    public void respondToGamepad() {
-        slideRespondToGamepad();
-        gripRespondToGamepad();
-    }
-
-    public void gripRespondToGamepad() {
-        if (gamepad.right_trigger > 0.5) {
-            gripPos = gripMin;
-        } else if (gamepad.left_trigger > 0.5) {
-            gripPos = gripMax;
-        }
-
-        gripPos = Maths.clamp(gripPos, gripMin, gripMax);
-
-        gripper1Servo.setPosition(1 - gripPos);
-        gripper2Servo.setPosition(gripPos);
-    }
-
-    public void slideRespondToGamepad(){
-        if (gamepad.a) {
-            armMotorSteps = 0;
-        } else if (gamepad.b) {
-            armMotorSteps = 86;
-        } else if (gamepad.x) {
-            armMotorSteps = 270;
-        } else if (gamepad.y) {
-            armMotorSteps = 460;
-        }
-
-        // TODO: check if this sensitivity is right
-        if (gamepad.dpad_up) {
-            armMotorSteps += 4;
-            dpadpressed = true;
-        } else if (gamepad.dpad_down) {
-            armMotorSteps -= 4;
-            dpadpressed = true;
-        } else if (dpadpressed) {
-            armMotorSteps = slideMotor.getCurrentPosition();
-            dpadpressed = false;
-        }
-
-        armMotorSteps = Maths.clamp(armMotorSteps, minHeight, maxHeight);
-
-        slideMotor.setTargetPosition(armMotorSteps);
-    }
-
-    public void grab() {
-        gripper1Servo.setPosition(1);
-        gripper2Servo.setPosition(0);
-    }
-
-    public void ungrab() {
-        gripper1Servo.setPosition(gripMin);
-        gripper2Servo.setPosition(1-gripMin);
-    }
-
-    public void setSlide(int steps) {
-        slideMotor.setTargetPosition(Maths.clamp(steps, minHeight, maxHeight));
-        while (linearOpMode.opModeIsActive() && slideMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/NewPoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/NewPoleDetection.java
deleted file mode 100644
index a395cc2..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/NewPoleDetection.java
+++ /dev/null
@@ -1,84 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class NewPoleDetection extends OpenCvPipeline {
-
-
-    private volatile boolean isPole = false;
-
-    // TOPLEFT anchor point for the bounding box
-    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(45, 60);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds  = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds  = new Scalar(255, 255, 150, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW  = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255);
-
-    // Percent and mat definitions
-    private double yelPercent;
-    private Mat yelMat = new Mat(), blurredMat = new Mat(), kernel = new Mat(), poleEdge = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        //Black and white
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
-
-
-        // Gets color specific values
-        yelPercent = Core.countNonZero(yelMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (yelPercent >= 60) {
-            isPole = true;
-        } else {
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    public boolean getPole() {
-        return isPole;
-    }
-    public double getPercent() {
-        return yelPercent;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/PoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/PoleDetection.java
deleted file mode 100644
index 088a4eb..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/PoleDetection.java
+++ /dev/null
@@ -1,100 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class PoleDetection extends OpenCvPipeline {
-    private volatile boolean isPole = false;
-
-    // TOPLEFT anchor point for the bounding box
-    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(45, 60);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds = new Scalar(255, 255, 150, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255);
-
-    // Percent and mat definitions
-    private double yelPercent;
-    private final Mat yelMat = new Mat();
-    private Mat blurredMat = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-
-        // Gets color specific values
-        yelPercent = Core.countNonZero(yelMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (yelPercent >= 60) {
-            isPole = true;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else {
-            isPole = false;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    public boolean getPole() {
-        return isPole;
-    }
-
-    public double getPercent() {
-        return yelPercent;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SingleJointGripperArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SingleJointGripperArm.java
deleted file mode 100644
index 12c4b83..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SingleJointGripperArm.java
+++ /dev/null
@@ -1,102 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.Util.Maths;
-
-public class SingleJointGripperArm {
-    private final Telemetry telemetry;
-    public final DcMotor armMotor;
-    public final Servo gripper1Servo;
-    public final Servo gripper2Servo;
-    public final Gamepad gamepad;
-    public final double gripMin;
-    public final double gripMax;
-    public final int armMin;
-    public final int armMax;
-    private int armMotorSteps = 0;
-    private double gripPos = 1;
-    private LinearOpMode linearOpMode;
-    private boolean dpadpressed = false;
-
-    public SingleJointGripperArm(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor armMotor, Servo gripper1Servo, Servo gripper2Servo, Gamepad gamepad, double gripMin, double gripMax, int armMin, int armMax) {
-        this.linearOpMode = linearOpMode;
-        this.telemetry = telemetry;
-        this.armMotor = armMotor;
-        this.gripper1Servo = gripper1Servo;
-        this.gripper2Servo = gripper2Servo;
-        this.gamepad = gamepad;
-        this.gripMin = gripMin;
-        this.gripMax = gripMax;
-        this.armMin = armMin;
-        this.armMax = armMax;
-    }
-
-    public void respondToGamepad() {
-        armRespondToGamepad();
-        gripRespondToGamepad();
-    }
-
-    public void gripRespondToGamepad() {
-        if (gamepad.right_trigger > 0.5) {
-            gripPos = gripMin;
-        } else if (gamepad.left_trigger > 0.5) {
-            gripPos = gripMax;
-        }
-
-        gripPos = Maths.clamp(gripPos, gripMin, gripMax);
-
-        gripper1Servo.setPosition(1 - gripPos);
-        gripper2Servo.setPosition(gripPos);
-    }
-
-    public void armRespondToGamepad() {
-        if (gamepad.a) {
-            armMotorSteps = 0;
-        } else if (gamepad.b) {
-            armMotorSteps = 86;
-        } else if (gamepad.x) {
-            armMotorSteps = 270;
-        } else if (gamepad.y) {
-            armMotorSteps = 460;
-        }
-
-        // TODO: check if this sensitivity is right
-        if (gamepad.dpad_up) {
-            armMotorSteps += 4;
-            dpadpressed = true;
-        } else if (gamepad.dpad_down) {
-            armMotorSteps -= 4;
-            dpadpressed = true;
-        } else if (dpadpressed) {
-            armMotorSteps = armMotor.getCurrentPosition();
-            dpadpressed = false;
-        }
-
-        armMotorSteps = Maths.clamp(armMotorSteps, armMin, armMax);
-
-        armMotor.setTargetPosition(armMotorSteps);
-    }
-
-    public void grab() {
-        gripper1Servo.setPosition(0);
-        gripper2Servo.setPosition(1);
-    }
-
-    public void ungrab() {
-        gripper1Servo.setPosition(1-gripMin);
-        gripper2Servo.setPosition(gripMin);
-    }
-
-    public void setArm(int steps) {
-        armMotor.setTargetPosition(Maths.clamp(steps, armMin, armMax));
-        while (linearOpMode.opModeIsActive() && armMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SleeveDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SleeveDetection.java
deleted file mode 100644
index 379c904..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/SleeveDetection.java
+++ /dev/null
@@ -1,134 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class SleeveDetection extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum ParkingPosition {
-        LEFT,
-        CENTER,
-        RIGHT
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(55, 120);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds = new Scalar(255, 255, 150, 255),
-            lower_cyan_bounds = new Scalar(0, 120, 120, 255),
-            upper_cyan_bounds = new Scalar(150, 255, 255, 255),
-            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
-            upper_magenta_bounds = new Scalar(255, 170, 255, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255),
-            MAGENTA = new Scalar(255, 0, 255);
-
-    // Percent and mat definitions
-    private double yelPercent, cyaPercent, magPercent;
-    private final Mat yelMat = new Mat();
-    private final Mat cyaMat = new Mat();
-    private final Mat magMat = new Mat();
-    private Mat blurredMat = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-    // Running variable storing the parking position
-    private volatile ParkingPosition position = ParkingPosition.LEFT;
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-        Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
-        Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
-
-        // Gets color specific values
-        yelPercent = Core.countNonZero(yelMat);
-        cyaPercent = Core.countNonZero(cyaMat);
-        magPercent = Core.countNonZero(magMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == yelPercent) {
-            position = ParkingPosition.LEFT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else if (maxPercent == cyaPercent) {
-            position = ParkingPosition.CENTER;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        } else if (maxPercent == magPercent) {
-            position = ParkingPosition.RIGHT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    MAGENTA,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        cyaMat.release();
-        magMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public ParkingPosition getPosition() {
-        return position;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/StrafeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/StrafeDrive.java
deleted file mode 100644
index 59ea3e6..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Subsystems/StrafeDrive.java
+++ /dev/null
@@ -1,176 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import org.firstinspires.ftc.teamcode.old.Util.Measure;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-public class StrafeDrive {
-    private final Telemetry telemetry;
-    public final DcMotor frontLeftMotor;
-    public final DcMotor frontRightMotor;
-    public final DcMotor backLeftMotor;
-    public final DcMotor backRightMotor;
-    public final LinearOpMode linearOpMode;
-    public final Gamepad gamepad;
-    public int flPos = 0;
-    public int frPos = 0;
-    public int blPos = 0;
-    public int brPos = 0;
-
-    public StrafeDrive(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor frontLeftMotor, DcMotor frontRightMotor, DcMotor backLeftMotor, DcMotor backRightMotor, Gamepad gamepad) {
-        this.telemetry = telemetry;
-        this.linearOpMode = linearOpMode;
-        this.frontLeftMotor = frontLeftMotor;
-        this.frontRightMotor = frontRightMotor;
-        this.backLeftMotor = backLeftMotor;
-        this.backRightMotor = backRightMotor;
-        this.gamepad = gamepad;
-    }
-
-
-    public void respondToGamepad() {
-
-        double forward = -gamepad.left_stick_y;
-        double strafe = gamepad.left_stick_x;
-        double rotate = gamepad.right_stick_x;
-        if (Math.abs(-gamepad.left_stick_y) < 0.01) {
-            forward = 0;
-        }
-
-        if (Math.abs(gamepad.left_stick_x) < 0.01) {
-            strafe = 0;
-        }
-
-        if (Math.abs(gamepad.right_stick_x) < 0.01) {
-            rotate = 0;
-        }
-        frontLeftMotor.setPower(forward + strafe + rotate);
-        backLeftMotor.setPower(forward - strafe + rotate);
-        frontRightMotor.setPower(forward + strafe - rotate);
-        backRightMotor.setPower(forward - strafe - rotate);
-    }
-
-    public void drive(int leftMove, int rightMove, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = leftMove;
-        frPos = rightMove;
-        blPos = leftMove;
-        brPos = rightMove;
-
-
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-    public void drivein(double leftinches, int rightinches, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = Measure.sTicks(leftinches);
-        frPos = Measure.sTicks(rightinches);
-        blPos = Measure.sTicks(leftinches);
-        brPos = Measure.sTicks(rightinches);
-
-
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-    public void strafe(int move, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = move;
-        frPos = move;
-        blPos = move * -1;
-        brPos = move * -1;
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-    public void strafein(double inches, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = Measure.sTicks(inches);
-        frPos = Measure.sTicks(inches);
-        blPos = Measure.sTicks(inches) * -1;
-        brPos = Measure.sTicks(inches) * -1;
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/CameraRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/CameraRobotTeleOp.java
deleted file mode 100644
index 9c4cac5..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/CameraRobotTeleOp.java
+++ /dev/null
@@ -1,80 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.TeleOp;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.PoleDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class CameraRobotTeleOp extends LinearOpMode {
-
-    PoleDetection poleDetection = new PoleDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        poleDetection = new PoleDetection();
-        camera.setPipeline(poleDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
-            @Override
-            public void onOpened() {
-                camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {
-            }
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("Is there a pole?: ", poleDetection.getPole());
-            telemetry.addData("Percent Yellow: ", poleDetection.getPercent());
-            telemetry.update();
-        }
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        waitForStart();
-
-        while (opModeIsActive()) {
-            gamepadPlusCamera(robot, poleDetection);
-            robot.singleJointGripperArm.respondToGamepad();
-            telemetry.addData("Is there a pole?: ", poleDetection.getPole());
-            telemetry.addData("Percent Yellow: ", poleDetection.getPercent());
-            telemetry.update();
-        }
-    }
-
-    private void gamepadPlusCamera(OldRobot robot, PoleDetection poleDetection) {
-        robot.backTankDrive.respondToGamepad();
-        if (robot.backTankDrive.gamepad.right_bumper) {
-            if (poleDetection.getPole()) {
-                robot.backTankDrive.backLeftMotor.setPower(0);
-                robot.backTankDrive.backRightMotor.setPower(0);
-            } else {
-                robot.backTankDrive.backLeftMotor.setPower(-0.25);
-                robot.backTankDrive.backRightMotor.setPower(0.25);
-            }
-        }
-        if (robot.backTankDrive.gamepad.left_bumper) {
-            if (poleDetection.getPole()) {
-                robot.backTankDrive.backLeftMotor.setPower(0);
-                robot.backTankDrive.backRightMotor.setPower(0);
-            } else {
-                robot.backTankDrive.backLeftMotor.setPower(0.25);
-                robot.backTankDrive.backRightMotor.setPower(-0.25);
-                //abcdefghijklmnopqrstuvwxyz
-                //The quick brown fox jumped over the lazy dog
-                //314159265358979326538
-            }
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/NewRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/NewRobotTeleOp.java
deleted file mode 100644
index be2ea0f..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/NewRobotTeleOp.java
+++ /dev/null
@@ -1,26 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.TeleOp;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.NewRobot;
-
-@Disabled
-public class NewRobotTeleOp extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        NewRobot robot = new NewRobot(this, hardwareMap, telemetry, gamepad1);
-
-        robot.strafeDrive.backLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.strafeDrive.frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.strafeDrive.backRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.strafeDrive.frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        waitForStart();
-
-        while (opModeIsActive()) {
-            robot.strafeDrive.respondToGamepad();
-            telemetry.update();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/OldRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/OldRobotTeleOp.java
deleted file mode 100644
index 7cf6393..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/TeleOp/OldRobotTeleOp.java
+++ /dev/null
@@ -1,22 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.TeleOp;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-
-@Disabled
-public class OldRobotTeleOp extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        waitForStart();
-
-        while (opModeIsActive()) {
-            robot.backTankDrive.respondToGamepad();
-            robot.singleJointGripperArm.respondToGamepad();
-            telemetry.update();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Maths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Maths.java
deleted file mode 100644
index a291046..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Maths.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Util;
-
-public class Maths {
-    public static float clamp(float val, float min, float max) {
-        if (val < min) {
-            return min;
-        } else if (val > max) {
-            return max;
-        }
-        return val;
-    }
-
-    public static int clamp(int val, int min, int max) {
-        if (val < min) {
-            return min;
-        } else if (val > max) {
-            return max;
-        }
-        return val;
-    }
-
-    public static double clamp(double val, double min, double max) {
-        if (val < min) {
-            return min;
-        } else if (val > max) {
-            return max;
-        }
-        return val;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Measure.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Measure.java
deleted file mode 100644
index 46227c9..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/Util/Measure.java
+++ /dev/null
@@ -1,12 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Util;
-
-public class Measure {
-
-    public static int dTicks(double inches) {
-        return (int)Math.round(inches * 54);
-    }
-
-    public static int sTicks(double inches) {
-        return (int)Math.round(inches * 56.25);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
similarity index 91%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
index 3827ed7..eb16ea1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
@@ -1,14 +1,14 @@
-package org.firstinspires.ftc.teamcode.auto;
+package org.firstinspires.ftc.teamcode.old.auto;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.RobotOpMode;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionRight;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionRight.ParkingPosition;
-import org.firstinspires.ftc.teamcode.subsystem.LinearSlide;
-import org.firstinspires.ftc.teamcode.subsystem.StrafeDrive;
-import org.firstinspires.ftc.teamcode.subsystem.Webcam;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRight;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRight.ParkingPosition;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
 
 @Autonomous(name = "Autonomous Right")
 public class AutoRight extends RobotOpMode {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTestRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
similarity index 92%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTestRight.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
index 4fd34ca..cbdf326 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTestRight.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
@@ -1,15 +1,15 @@
-package org.firstinspires.ftc.teamcode.auto;
+package org.firstinspires.ftc.teamcode.old.auto;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.RobotOpMode;
-import org.firstinspires.ftc.teamcode.pipeline.PoleDetection;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew.ParkingPosition;
-import org.firstinspires.ftc.teamcode.subsystem.LinearSlide;
-import org.firstinspires.ftc.teamcode.subsystem.StrafeDrive;
-import org.firstinspires.ftc.teamcode.subsystem.Webcam;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.PoleDetection;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNew;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNew.ParkingPosition;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
 
 @Autonomous(name = "Autonomous Test Right")
 public class AutoTestRight extends RobotOpMode {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/BoeDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetection.java
similarity index 99%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/BoeDetection.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetection.java
index d225651..9c8342d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/BoeDetection.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetection.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.pipeline;
+package org.firstinspires.ftc.teamcode.old.pipeline;
 
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/PoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetection.java
similarity index 98%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/PoleDetection.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetection.java
index 0ceddc2..4070cd2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/PoleDetection.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetection.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.pipeline;
+package org.firstinspires.ftc.teamcode.old.pipeline;
 
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionNew.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNew.java
similarity index 98%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionNew.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNew.java
index ca4a385..b17ebe3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionNew.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNew.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.pipeline;
+package org.firstinspires.ftc.teamcode.old.pipeline;
 
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRight.java
similarity index 98%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionRight.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRight.java
index fdf28fb..0a5d7d9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionRight.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRight.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.pipeline;
+package org.firstinspires.ftc.teamcode.old.pipeline;
 
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
similarity index 97%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LinearSlide.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
index 203e592..127f4f1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
@@ -1,11 +1,11 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.old.subsystem;
 
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.Range;
 
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
-import org.firstinspires.ftc.teamcode.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
 
 public class LinearSlide extends Subsystem {
     // Useful Constants
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/StrafeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
similarity index 99%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/StrafeDrive.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
index c958444..18a6261 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/StrafeDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.old.subsystem;
 
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -7,7 +7,7 @@
 import com.qualcomm.robotcore.util.Range;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.teamcode.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
 
 public class StrafeDrive extends Subsystem {
     public IMU imu;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
similarity index 63%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/Subsystem.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
index 50eb4c9..20ae19f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/Subsystem.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
@@ -1,6 +1,6 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.old.subsystem;
 
-import org.firstinspires.ftc.teamcode.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
 
 abstract public class Subsystem {
     protected final RobotOpMode opMode;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/Webcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
similarity index 92%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/Webcam.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
index 1bba7fe..81096e9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/Webcam.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
@@ -1,7 +1,7 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.old.subsystem;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
 import org.openftc.easyopencv.OpenCvCamera;
 import org.openftc.easyopencv.OpenCvCameraFactory;
 import org.openftc.easyopencv.OpenCvCameraRotation;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
new file mode 100644
index 0000000..0af5e3e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
@@ -0,0 +1,226 @@
+/* Copyright (c) 2017-2020 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.old.teleop;
+
+import android.app.Activity;
+import android.graphics.Color;
+import android.view.View;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/**
+ * This is an example LinearOpMode that shows how to use a color sensor in a generic
+ * way, regardless of which particular make or model of color sensor is used. The Op Mode
+ * assumes that the color sensor is configured with a name of "sensor_color".
+ * <p>
+ * There will be some variation in the values measured depending on the specific sensor you are using.
+ * <p>
+ * You can increase the gain (a multiplier to make the sensor report higher values) by holding down
+ * the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad.
+ * <p>
+ * If the color sensor has a light which is controllable from software, you can use the X button on
+ * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have
+ * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2.
+ * <p>
+ * If the color sensor also supports short-range distance measurements (usually via an infrared
+ * proximity sensor), the reported distance will be written to telemetry. As of September 2020,
+ * the only color sensors that support this are the ones from REV Robotics. These infrared proximity
+ * sensor measurements are only useful at very small distances, and are sensitive to ambient light
+ * and surface reflectivity. You should use a different sensor if you need precise distance measurements.
+ * <p>
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this Op Mode to the Driver Station OpMode list
+ */
+@TeleOp(name = "Sensor: Color", group = "Sensor")
+public class SensorColor extends LinearOpMode {
+
+    /**
+     * The colorSensor field will contain a reference to our color sensor hardware object
+     */
+    NormalizedColorSensor colorSensor;
+
+    /**
+     * The relativeLayout field is used to aid in providing interesting visual feedback
+     * in this sample application; you probably *don't* need this when you use a color sensor on your
+     * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller.
+     */
+    View relativeLayout;
+
+    /**
+     * The runOpMode() method is the root of this Op Mode, as it is in all LinearOpModes.
+     * Our implementation here, though is a bit unusual: we've decided to put all the actual work
+     * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is
+     * that in this sample we're changing the background color of the robot controller screen as the
+     * Op Mode runs, and we want to be able to *guarantee* that we restore it to something reasonable
+     * and palatable when the Op Mode ends. The simplest way to do that is to use a try...finally
+     * block around the main, core logic, and an easy way to make that all clear was to separate
+     * the former from the latter in separate methods.
+     */
+    @Override
+    public void runOpMode() {
+
+        // Get a reference to the RelativeLayout so we can later change the background
+        // color of the Robot Controller app to match the hue detected by the RGB sensor.
+        int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
+        relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);
+
+        try {
+            runSample(); // actually execute the sample
+        } finally {
+            // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off
+            // as pure white, but it's too much work to dig out what actually was used, and this is good
+            // enough to at least make the screen reasonable again.
+            // Set the panel back to the default color
+            relativeLayout.post(new Runnable() {
+                public void run() {
+                    relativeLayout.setBackgroundColor(Color.WHITE);
+                }
+            });
+        }
+    }
+
+    protected void runSample() {
+        // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the
+        // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3)
+        // can give very low values (depending on the lighting conditions), which only use a small part
+        // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions,
+        // you should use a smaller gain than in dark conditions. If your gain is too high, all of the
+        // colors will report at or near 1, and you won't be able to determine what color you are
+        // actually looking at. For this reason, it's better to err on the side of a lower gain
+        // (but always greater than  or equal to 1).
+        float gain = 2;
+
+        // Once per loop, we will update this hsvValues array. The first element (0) will contain the
+        // hue, the second element (1) will contain the saturation, and the third element (2) will
+        // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
+        // for an explanation of HSV color.
+        final float[] hsvValues = new float[3];
+
+        // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current
+        // state of the X button on the gamepad
+        boolean xButtonPreviouslyPressed = false;
+        boolean xButtonCurrentlyPressed = false;
+
+        // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over
+        // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while
+        // the values you get from ColorSensor are dependent on the specific sensor you're using.
+        colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
+
+        // If possible, turn the light on in the beginning (it might already be on anyway,
+        // we just make sure it is if we can).
+        if (colorSensor instanceof SwitchableLight) {
+            ((SwitchableLight) colorSensor).enableLight(true);
+        }
+
+        // Wait for the start button to be pressed.
+        waitForStart();
+
+        // Loop until we are asked to stop
+        while (opModeIsActive()) {
+            // Explain basic gain information via telemetry
+            telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n");
+            telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n");
+
+            // Update the gain value if either of the A or B gamepad buttons is being held
+            if (gamepad1.a) {
+                // Only increase the gain by a small amount, since this loop will occur multiple times per second.
+                gain += 0.005;
+            } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful.
+                gain -= 0.005;
+            }
+
+            // Show the gain value via telemetry
+            telemetry.addData("Gain", gain);
+
+            // Tell the sensor our desired gain value (normally you would do this during initialization,
+            // not during the loop)
+            colorSensor.setGain(gain);
+
+            // Check the status of the X button on the gamepad
+            xButtonCurrentlyPressed = gamepad1.x;
+
+            // If the button state is different than what it was, then act
+            if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) {
+                // If the button is (now) down, then toggle the light
+                if (xButtonCurrentlyPressed) {
+                    if (colorSensor instanceof SwitchableLight) {
+                        SwitchableLight light = (SwitchableLight) colorSensor;
+                        light.enableLight(!light.isLightOn());
+                    }
+                }
+            }
+            xButtonPreviouslyPressed = xButtonCurrentlyPressed;
+
+            // Get the normalized colors from the sensor
+            NormalizedRGBA colors = colorSensor.getNormalizedColors();
+
+            /* Use telemetry to display feedback on the driver station. We show the red, green, and blue
+             * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent
+             * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
+             * for an explanation of HSV color. */
+
+            // Update the hsvValues array by passing it to Color.colorToHSV()
+            Color.colorToHSV(colors.toColor(), hsvValues);
+
+            telemetry.addLine()
+                    .addData("Red", "%.3f", colors.red)
+                    .addData("Green", "%.3f", colors.green)
+                    .addData("Blue", "%.3f", colors.blue);
+            telemetry.addLine()
+                    .addData("Hue", "%.3f", hsvValues[0])
+                    .addData("Saturation", "%.3f", hsvValues[1])
+                    .addData("Value", "%.3f", hsvValues[2]);
+            telemetry.addData("Alpha", "%.3f", colors.alpha);
+
+            /* If this color sensor also has a distance sensor, display the measured distance.
+             * Note that the reported distance is only useful at very close range, and is impacted by
+             * ambient light and surface reflectivity. */
+            if (colorSensor instanceof DistanceSensor) {
+                telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM));
+            }
+
+            telemetry.update();
+
+            // Change the Robot Controller's background color to match the color detected by the color sensor.
+            relativeLayout.post(new Runnable() {
+                public void run() {
+                    relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues));
+                }
+            });
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
new file mode 100644
index 0000000..d714b78
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
@@ -0,0 +1,221 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.util.LoggingUtil;
+import org.firstinspires.ftc.teamcode.util.RegressionUtil;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/*
+ * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
+ * outline of the procedure:
+ *   1. Slowly ramp the motor power and record encoder values along the way.
+ *   2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
+ *      and an optional intercept (kStatic).
+ *   3. Accelerate the robot (apply constant power) and record the encoder counts.
+ *   4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
+ *      regression.
+ */
+@Config
+@Autonomous(group = "drive")
+public class AutomaticFeedforwardTuner extends LinearOpMode {
+    public static double MAX_POWER = 0.7;
+    public static double DISTANCE = 100; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (RUN_USING_ENCODER) {
+            RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+                    "when using the built-in drive motor velocity PID.");
+        }
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        NanoClock clock = NanoClock.system();
+
+        telemetry.addLine("Press play to begin the feedforward tuning routine");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Would you like to fit kStatic?");
+        telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+        telemetry.update();
+
+        boolean fitIntercept = false;
+        while (!isStopRequested()) {
+            if (gamepad1.y) {
+                fitIntercept = true;
+                while (!isStopRequested() && gamepad1.y) {
+                    idle();
+                }
+                break;
+            } else if (gamepad1.b) {
+                while (!isStopRequested() && gamepad1.b) {
+                    idle();
+                }
+                break;
+            }
+            idle();
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine(Misc.formatInvariant(
+                "Place your robot on the field with at least %.2f in of room in front", DISTANCE));
+        telemetry.addLine("Press (Y/Δ) to begin");
+        telemetry.update();
+
+        while (!isStopRequested() && !gamepad1.y) {
+            idle();
+        }
+        while (!isStopRequested() && gamepad1.y) {
+            idle();
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        double maxVel = rpmToVelocity(MAX_RPM);
+        double finalVel = MAX_POWER * maxVel;
+        double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
+        double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
+
+        List<Double> timeSamples = new ArrayList<>();
+        List<Double> positionSamples = new ArrayList<>();
+        List<Double> powerSamples = new ArrayList<>();
+
+        drive.setPoseEstimate(new Pose2d());
+
+        double startTime = clock.seconds();
+        while (!isStopRequested()) {
+            double elapsedTime = clock.seconds() - startTime;
+            if (elapsedTime > rampTime) {
+                break;
+            }
+            double vel = accel * elapsedTime;
+            double power = vel / maxVel;
+
+            timeSamples.add(elapsedTime);
+            positionSamples.add(drive.getPoseEstimate().getX());
+            powerSamples.add(power);
+
+            drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
+            drive.updatePoseEstimate();
+        }
+        drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+        RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
+                timeSamples, positionSamples, powerSamples, fitIntercept,
+                LoggingUtil.getLogFile(Misc.formatInvariant(
+                        "DriveRampRegression-%d.csv", System.currentTimeMillis())));
+
+        telemetry.clearAll();
+        telemetry.addLine("Quasi-static ramp up test complete");
+        if (fitIntercept) {
+            telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
+                    rampResult.kV, rampResult.kStatic, rampResult.rSquare));
+        } else {
+            telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
+                    rampResult.kStatic, rampResult.rSquare));
+        }
+        telemetry.addLine("Would you like to fit kA?");
+        telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+        telemetry.update();
+
+        boolean fitAccelFF = false;
+        while (!isStopRequested()) {
+            if (gamepad1.y) {
+                fitAccelFF = true;
+                while (!isStopRequested() && gamepad1.y) {
+                    idle();
+                }
+                break;
+            } else if (gamepad1.b) {
+                while (!isStopRequested() && gamepad1.b) {
+                    idle();
+                }
+                break;
+            }
+            idle();
+        }
+
+        if (fitAccelFF) {
+            telemetry.clearAll();
+            telemetry.addLine("Place the robot back in its starting position");
+            telemetry.addLine("Press (Y/Δ) to continue");
+            telemetry.update();
+
+            while (!isStopRequested() && !gamepad1.y) {
+                idle();
+            }
+            while (!isStopRequested() && gamepad1.y) {
+                idle();
+            }
+
+            telemetry.clearAll();
+            telemetry.addLine("Running...");
+            telemetry.update();
+
+            double maxPowerTime = DISTANCE / maxVel;
+
+            timeSamples.clear();
+            positionSamples.clear();
+            powerSamples.clear();
+
+            drive.setPoseEstimate(new Pose2d());
+            drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
+
+            startTime = clock.seconds();
+            while (!isStopRequested()) {
+                double elapsedTime = clock.seconds() - startTime;
+                if (elapsedTime > maxPowerTime) {
+                    break;
+                }
+
+                timeSamples.add(elapsedTime);
+                positionSamples.add(drive.getPoseEstimate().getX());
+                powerSamples.add(MAX_POWER);
+
+                drive.updatePoseEstimate();
+            }
+            drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+            RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
+                    timeSamples, positionSamples, powerSamples, rampResult,
+                    LoggingUtil.getLogFile(Misc.formatInvariant(
+                            "DriveAccelRegression-%d.csv", System.currentTimeMillis())));
+
+            telemetry.clearAll();
+            telemetry.addLine("Constant power test complete");
+            telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
+                    accelResult.kA, accelResult.rSquare));
+            telemetry.update();
+        }
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
new file mode 100644
index 0000000..807fe6b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin moving forward and
+ * backward. You should observe the target position (green) and your pose estimate (blue) and adjust
+ * your follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ *
+ * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
+ * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
+ */
+@Config
+@Autonomous(group = "drive")
+public class BackAndForth extends LinearOpMode {
+
+    public static double DISTANCE = 50;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
+                .forward(DISTANCE)
+                .build();
+
+        Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
+                .back(DISTANCE)
+                .build();
+
+        waitForStart();
+
+        while (opModeIsActive() && !isStopRequested()) {
+            drive.followTrajectory(trajectoryForward);
+            drive.followTrajectory(trajectoryBackward);
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
new file mode 100644
index 0000000..0fd7f3a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
@@ -0,0 +1,171 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.List;
+
+/*
+ * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
+ * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
+ * important as the positional parameters. Like the other manual tuning routines, this op mode
+ * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
+ * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
+ * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
+ * connected, start the program, and your robot will begin moving forward and backward according to
+ * a motion profile. Your job is to graph the velocity errors over time and adjust the PID
+ * coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
+ * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
+ * MOTOR_VELO_PID field.
+ *
+ * Recommended tuning process:
+ *
+ * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
+ *    mitigate oscillations.
+ * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
+ * 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Autonomous(group = "drive")
+public class DriveVelocityPIDTuner extends LinearOpMode {
+    public static double DISTANCE = 72; // in
+
+    enum Mode {
+        DRIVER_MODE,
+        TUNING_MODE
+    }
+
+    private static MotionProfile generateProfile(boolean movingForward) {
+        MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+        MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+        return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+    }
+
+    @Override
+    public void runOpMode() {
+        if (!RUN_USING_ENCODER) {
+            RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
+                    "PID is not in use", getClass().getSimpleName());
+        }
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        Mode mode = Mode.TUNING_MODE;
+
+        double lastKp = MOTOR_VELO_PID.p;
+        double lastKi = MOTOR_VELO_PID.i;
+        double lastKd = MOTOR_VELO_PID.d;
+        double lastKf = MOTOR_VELO_PID.f;
+
+        drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+        NanoClock clock = NanoClock.system();
+
+        telemetry.addLine("Ready!");
+        telemetry.update();
+        telemetry.clearAll();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        boolean movingForwards = true;
+        MotionProfile activeProfile = generateProfile(true);
+        double profileStart = clock.seconds();
+
+
+        while (!isStopRequested()) {
+            telemetry.addData("mode", mode);
+
+            switch (mode) {
+                case TUNING_MODE:
+                    if (gamepad1.y) {
+                        mode = Mode.DRIVER_MODE;
+                        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                    }
+
+                    // calculate and set the motor power
+                    double profileTime = clock.seconds() - profileStart;
+
+                    if (profileTime > activeProfile.duration()) {
+                        // generate a new profile
+                        movingForwards = !movingForwards;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    MotionState motionState = activeProfile.get(profileTime);
+                    double targetPower = kV * motionState.getV();
+                    drive.setDrivePower(new Pose2d(targetPower, 0, 0));
+
+                    List<Double> velocities = drive.getWheelVelocities();
+
+                    // update telemetry
+                    telemetry.addData("targetVelocity", motionState.getV());
+                    for (int i = 0; i < velocities.size(); i++) {
+                        telemetry.addData("measuredVelocity" + i, velocities.get(i));
+                        telemetry.addData(
+                                "error" + i,
+                                motionState.getV() - velocities.get(i)
+                        );
+                    }
+                    break;
+                case DRIVER_MODE:
+                    if (gamepad1.b) {
+                        drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+                        mode = Mode.TUNING_MODE;
+                        movingForwards = true;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    drive.setWeightedDrivePower(
+                            new Pose2d(
+                                    -gamepad1.left_stick_y,
+                                    -gamepad1.left_stick_x,
+                                    -gamepad1.right_stick_x
+                            )
+                    );
+                    break;
+            }
+
+            if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
+                    || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
+                drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+                lastKp = MOTOR_VELO_PID.p;
+                lastKi = MOTOR_VELO_PID.i;
+                lastKd = MOTOR_VELO_PID.d;
+                lastKf = MOTOR_VELO_PID.f;
+            }
+
+            telemetry.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
new file mode 100644
index 0000000..cfb7ad6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin driving in a square.
+ * You should observe the target position (green) and your pose estimate (blue) and adjust your
+ * follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ */
+@Config
+@Autonomous(group = "drive")
+public class FollowerPIDTuner extends LinearOpMode {
+    public static double DISTANCE = 48; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
+
+        drive.setPoseEstimate(startPose);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (!isStopRequested()) {
+            TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .build();
+            drive.followTrajectorySequence(trajSeq);
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
new file mode 100644
index 0000000..73ac2eb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/**
+ * This is a simple teleop routine for testing localization. Drive the robot around like a normal
+ * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
+ * errors are not out of the ordinary, especially with sudden drive motions). The goal of this
+ * exercise is to ascertain whether the localizer has been configured properly (note: the pure
+ * encoder localizer heading may be significantly off if the track width has not been tuned).
+ */
+@TeleOp(group = "drive")
+public class LocalizationTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        waitForStart();
+
+        while (!isStopRequested()) {
+            drive.setWeightedDrivePower(
+                    new Pose2d(
+                            -gamepad1.left_stick_y,
+                            -gamepad1.left_stick_x,
+                            -gamepad1.right_stick_x
+                    )
+            );
+
+            drive.update();
+
+            Pose2d poseEstimate = drive.getPoseEstimate();
+            telemetry.addData("x", poseEstimate.getX());
+            telemetry.addData("y", poseEstimate.getY());
+            telemetry.addData("heading", poseEstimate.getHeading());
+            telemetry.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
new file mode 100644
index 0000000..8aa8b06
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
@@ -0,0 +1,147 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/*
+ * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
+ * tuning these coefficients is just as important as the positional parameters. Like the other
+ * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
+ * you are using the Control Hub. Once you've successfully connected, start the program, and your
+ * robot will begin moving forward and backward according to a motion profile. Your job is to graph
+ * the velocity errors over time and adjust the feedforward coefficients. Once you've found a
+ * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Autonomous(group = "drive")
+public class ManualFeedforwardTuner extends LinearOpMode {
+    public static double DISTANCE = 72; // in
+
+    private FtcDashboard dashboard = FtcDashboard.getInstance();
+
+    private SampleMecanumDrive drive;
+
+    enum Mode {
+        DRIVER_MODE,
+        TUNING_MODE
+    }
+
+    private Mode mode;
+
+    private static MotionProfile generateProfile(boolean movingForward) {
+        MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+        MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+        return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+    }
+
+    @Override
+    public void runOpMode() {
+        if (RUN_USING_ENCODER) {
+            RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+                    "when using the built-in drive motor velocity PID.");
+        }
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
+
+        drive = new SampleMecanumDrive(hardwareMap);
+
+        mode = Mode.TUNING_MODE;
+
+        NanoClock clock = NanoClock.system();
+
+        telemetry.addLine("Ready!");
+        telemetry.update();
+        telemetry.clearAll();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        boolean movingForwards = true;
+        MotionProfile activeProfile = generateProfile(true);
+        double profileStart = clock.seconds();
+
+
+        while (!isStopRequested()) {
+            telemetry.addData("mode", mode);
+
+            switch (mode) {
+                case TUNING_MODE:
+                    if (gamepad1.y) {
+                        mode = Mode.DRIVER_MODE;
+                    }
+
+                    // calculate and set the motor power
+                    double profileTime = clock.seconds() - profileStart;
+
+                    if (profileTime > activeProfile.duration()) {
+                        // generate a new profile
+                        movingForwards = !movingForwards;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    MotionState motionState = activeProfile.get(profileTime);
+                    double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
+
+                    drive.setDrivePower(new Pose2d(targetPower, 0, 0));
+                    drive.updatePoseEstimate();
+
+                    Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+                    double currentVelo = poseVelo.getX();
+
+                    // update telemetry
+                    telemetry.addData("targetVelocity", motionState.getV());
+                    telemetry.addData("measuredVelocity", currentVelo);
+                    telemetry.addData("error", motionState.getV() - currentVelo);
+                    break;
+                case DRIVER_MODE:
+                    if (gamepad1.b) {
+                        mode = Mode.TUNING_MODE;
+                        movingForwards = true;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    drive.setWeightedDrivePower(
+                            new Pose2d(
+                                    -gamepad1.left_stick_y,
+                                    -gamepad1.left_stick_x,
+                                    -gamepad1.right_stick_x
+                            )
+                    );
+                    break;
+            }
+
+            telemetry.update();
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
new file mode 100644
index 0000000..79c9df2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
@@ -0,0 +1,73 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
+ * <p>
+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds.
+ * <p>
+ * Further fine tuning of MAX_ANG_VEL may be desired.
+ */
+
+@Config
+@Autonomous(group = "drive")
+public class MaxAngularVeloTuner extends LinearOpMode {
+    public static double RUNTIME = 4.0;
+
+    private ElapsedTime timer;
+    private double maxAngVelocity = 0.0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
+        telemetry.addLine("Please ensure you have enough space cleared.");
+        telemetry.addLine("");
+        telemetry.addLine("Press start when ready.");
+        telemetry.update();
+
+        waitForStart();
+
+        telemetry.clearAll();
+        telemetry.update();
+
+        drive.setDrivePower(new Pose2d(0, 0, 1));
+        timer = new ElapsedTime();
+
+        while (!isStopRequested() && timer.seconds() < RUNTIME) {
+            drive.updatePoseEstimate();
+
+            Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+            maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
+        }
+
+        drive.setDrivePower(new Pose2d());
+
+        telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
+        telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
+        telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
+        telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
+        telemetry.update();
+
+        while (!isStopRequested()) idle();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
new file mode 100644
index 0000000..ccc4118
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
@@ -0,0 +1,84 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum velocity your bot can achieve under load. It
+ * will also calculate the effective kF value for your velocity PID.
+ * <p>
+ * Upon pressing start, your bot will run at max power for RUNTIME seconds.
+ * <p>
+ * Further fine tuning of kF may be desired.
+ */
+@Config
+@Autonomous(group = "drive")
+public class MaxVelocityTuner extends LinearOpMode {
+    public static double RUNTIME = 2.0;
+
+    private ElapsedTime timer;
+    private double maxVelocity = 0.0;
+
+    private VoltageSensor batteryVoltageSensor;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
+        telemetry.addLine("Please ensure you have enough space cleared.");
+        telemetry.addLine("");
+        telemetry.addLine("Press start when ready.");
+        telemetry.update();
+
+        waitForStart();
+
+        telemetry.clearAll();
+        telemetry.update();
+
+        drive.setDrivePower(new Pose2d(1, 0, 0));
+        timer = new ElapsedTime();
+
+        while (!isStopRequested() && timer.seconds() < RUNTIME) {
+            drive.updatePoseEstimate();
+
+            Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+            maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
+        }
+
+        drive.setDrivePower(new Pose2d());
+
+        double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
+
+        telemetry.addData("Max Velocity", maxVelocity);
+        telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
+        telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
+        telemetry.update();
+
+        while (!isStopRequested() && opModeIsActive()) idle();
+    }
+
+    private double veloInchesToTicks(double inchesPerSec) {
+        return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
new file mode 100644
index 0000000..0a3b688
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/**
+ * This is a simple teleop routine for debugging your motor configuration.
+ * Pressing each of the buttons will power its respective motor.
+ * <p>
+ * Button Mappings:
+ * <p>
+ * Xbox/PS4 Button - Motor
+ * X / ▢         - Front Left
+ * Y / Δ         - Front Right
+ * B / O         - Rear  Right
+ * A / X         - Rear  Left
+ * The buttons are mapped to match the wheels spatially if you
+ * were to rotate the gamepad 45deg°. x/square is the front left
+ * ________        and each button corresponds to the wheel as you go clockwise
+ * / ______ \
+ * ------------.-'   _  '-..+              Front of Bot
+ * /   _  ( Y )  _  \                  ^
+ * |  ( X )  _  ( B ) |     Front Left   \    Front Right
+ * ___  '.      ( A )     /|       Wheel       \      Wheel
+ * .'    '.    '-._____.-'  .'       (x/▢)        \     (Y/Δ)
+ * |       |                 |                      \
+ * '.___.' '.               |          Rear Left    \   Rear Right
+ * '.             /             Wheel       \    Wheel
+ * \.          .'              (A/X)        \   (B/O)
+ * \________/
+ * <p>
+ * Uncomment the @Disabled tag below to use this opmode.
+ */
+@Disabled
+@Config
+@TeleOp(group = "drive")
+public class MotorDirectionDebugger extends LinearOpMode {
+    public static double MOTOR_POWER = 0.7;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        telemetry.addLine("Press play to begin the debugging opmode");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+
+        while (!isStopRequested()) {
+            telemetry.addLine("Press each button to turn on its respective motor");
+            telemetry.addLine();
+            telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
+            telemetry.addLine();
+
+            if (gamepad1.x) {
+                drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
+                telemetry.addLine("Running Motor: Front Left");
+            } else if (gamepad1.y) {
+                drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
+                telemetry.addLine("Running Motor: Front Right");
+            } else if (gamepad1.b) {
+                drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
+                telemetry.addLine("Running Motor: Rear Right");
+            } else if (gamepad1.a) {
+                drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
+                telemetry.addLine("Running Motor: Rear Left");
+            } else {
+                drive.setMotorPowers(0, 0, 0, 0);
+                telemetry.addLine("Running Motor: None");
+            }
+
+            telemetry.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
new file mode 100644
index 0000000..506c3b3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is an example of a more complex path to really test the tuning.
+ */
+@Autonomous(group = "drive")
+public class SplineTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        Trajectory traj = drive.trajectoryBuilder(new Pose2d())
+                .splineTo(new Vector2d(30, 30), 0)
+                .build();
+
+        drive.followTrajectory(traj);
+
+        sleep(2000);
+
+        drive.followTrajectory(
+                drive.trajectoryBuilder(traj.end(), true)
+                        .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+                        .build()
+        );
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
new file mode 100644
index 0000000..90ff4d7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Autonomous(group = "drive")
+public class StrafeTest extends LinearOpMode {
+    public static double DISTANCE = 60; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+                .strafeRight(DISTANCE)
+                .build();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.followTrajectory(trajectory);
+
+        Pose2d poseEstimate = drive.getPoseEstimate();
+        telemetry.addData("finalX", poseEstimate.getX());
+        telemetry.addData("finalY", poseEstimate.getY());
+        telemetry.addData("finalHeading", poseEstimate.getHeading());
+        telemetry.update();
+
+        while (!isStopRequested() && opModeIsActive()) ;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
new file mode 100644
index 0000000..bee34af
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Autonomous(group = "drive")
+public class StraightTest extends LinearOpMode {
+    public static double DISTANCE = 60; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+                .forward(DISTANCE)
+                .build();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.followTrajectory(trajectory);
+
+        Pose2d poseEstimate = drive.getPoseEstimate();
+        telemetry.addData("finalX", poseEstimate.getX());
+        telemetry.addData("finalY", poseEstimate.getY());
+        telemetry.addData("finalHeading", poseEstimate.getHeading());
+        telemetry.update();
+
+        while (!isStopRequested() && opModeIsActive()) ;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
new file mode 100644
index 0000000..b852d3b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
@@ -0,0 +1,88 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This routine determines the effective track width. The procedure works by executing a point turn
+ * with a given angle and measuring the difference between that angle and the actual angle (as
+ * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
+ * given angle / actual angle gives a multiplicative adjustment to the estimated track width
+ * (effective track width = estimated track width * given angle / actual angle). The routine repeats
+ * this procedure a few times and averages the values for additional accuracy. Note: a relatively
+ * accurate track width estimate is important or else the angular constraints will be thrown off.
+ */
+@Config
+@Autonomous(group = "drive")
+public class TrackWidthTuner extends LinearOpMode {
+    public static double ANGLE = 180; // deg
+    public static int NUM_TRIALS = 5;
+    public static int DELAY = 1000; // ms
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        // TODO: if you haven't already, set the localizer to something that doesn't depend on
+        // drive encoders for computing the heading
+
+        telemetry.addLine("Press play to begin the track width tuner routine");
+        telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
+        for (int i = 0; i < NUM_TRIALS; i++) {
+            drive.setPoseEstimate(new Pose2d());
+
+            // it is important to handle heading wraparounds
+            double headingAccumulator = 0;
+            double lastHeading = 0;
+
+            drive.turnAsync(Math.toRadians(ANGLE));
+
+            while (!isStopRequested() && drive.isBusy()) {
+                double heading = drive.getPoseEstimate().getHeading();
+                headingAccumulator += Angle.normDelta(heading - lastHeading);
+                lastHeading = heading;
+
+                drive.update();
+            }
+
+            double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
+            trackWidthStats.add(trackWidth);
+
+            sleep(DELAY);
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Tuning complete");
+        telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
+                trackWidthStats.getMean(),
+                trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+        telemetry.update();
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
new file mode 100644
index 0000000..8be29de
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * This routine determines the effective forward offset for the lateral tracking wheel.
+ * The procedure executes a point turn at a given angle for a certain number of trials,
+ * along with a specified delay in milliseconds. The purpose of this is to track the
+ * change in the y position during the turn. The offset, or distance, of the lateral tracking
+ * wheel from the center or rotation allows the wheel to spin during a point turn, leading
+ * to an incorrect measurement for the y position. This creates an arc around around
+ * the center of rotation with an arc length of change in y and a radius equal to the forward
+ * offset. We can compute this offset by calculating (change in y position) / (change in heading)
+ * which returns the radius if the angle (change in heading) is in radians. This is based
+ * on the arc length formula of length = theta * radius.
+ * <p>
+ * To run this routine, simply adjust the desired angle and specify the number of trials
+ * and the desired delay. Then, run the procedure. Once it finishes, it will print the
+ * average of all the calculated forward offsets derived from the calculation. This calculated
+ * forward offset is then added onto the current forward offset to produce an overall estimate
+ * for the forward offset. You can run this procedure as many times as necessary until a
+ * satisfactory result is produced.
+ */
+@Config
+@Autonomous(group = "drive")
+public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
+    public static double ANGLE = 180; // deg
+    public static int NUM_TRIALS = 5;
+    public static int DELAY = 1000; // ms
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+        }
+
+        telemetry.addLine("Press play to begin the forward offset tuner");
+        telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
+        for (int i = 0; i < NUM_TRIALS; i++) {
+            drive.setPoseEstimate(new Pose2d());
+
+            // it is important to handle heading wraparounds
+            double headingAccumulator = 0;
+            double lastHeading = 0;
+
+            drive.turnAsync(Math.toRadians(ANGLE));
+
+            while (!isStopRequested() && drive.isBusy()) {
+                double heading = drive.getPoseEstimate().getHeading();
+                headingAccumulator += Angle.norm(heading - lastHeading);
+                lastHeading = heading;
+
+                drive.update();
+            }
+
+            double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
+                    drive.getPoseEstimate().getY() / headingAccumulator;
+            forwardOffsetStats.add(forwardOffset);
+
+            sleep(DELAY);
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Tuning complete");
+        telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
+                forwardOffsetStats.getMean(),
+                forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+        telemetry.update();
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
new file mode 100644
index 0000000..22c8224
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
+ * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
+ * wheels.
+ * <p>
+ * Tuning Routine:
+ * <p>
+ * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
+ * measured value. This need only be an estimated value as you will be tuning it anyways.
+ * <p>
+ * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
+ * similar mark right below the indicator on your bot. This will be your reference point to
+ * ensure you've turned exactly 360°.
+ * <p>
+ * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
+ * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
+ * if you are using the Control Hub.
+ * Ensure the field is showing (select the field view in top right of the page).
+ * <p>
+ * 4. Press play to begin the tuning routine.
+ * <p>
+ * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
+ * <p>
+ * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
+ * <p>
+ * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
+ * on the bot and on the ground you created earlier should be lined up.
+ * <p>
+ * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
+ * StandardTrackingWheelLocalizer.java class.
+ * <p>
+ * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
+ * yourself. Read the heading output and follow the advice stated in the note below to manually
+ * nudge the values yourself.
+ * <p>
+ * Note:
+ * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
+ * a line from the circumference to the center should be present, representing the bot. The line
+ * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
+ * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
+ * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
+ * actual bot, the LATERAL_DISTANCE should be increased.
+ * <p>
+ * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
+ * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
+ * effective center of rotation. You can ignore this effect. The center of rotation will be offset
+ * slightly but your heading will still be fine. This does not affect your overall tracking
+ * precision. The heading should still line up.
+ */
+@Config
+@TeleOp(group = "drive")
+public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
+    public static int NUM_TURNS = 10;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+        }
+
+        telemetry.addLine("Prior to beginning the routine, please read the directions "
+                + "located in the comments of the opmode file.");
+        telemetry.addLine("Press play to begin the tuning routine.");
+        telemetry.addLine("");
+        telemetry.addLine("Press Y/△ to stop the routine.");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.update();
+
+        double headingAccumulator = 0;
+        double lastHeading = 0;
+
+        boolean tuningFinished = false;
+
+        while (!isStopRequested() && !tuningFinished) {
+            Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
+            drive.setDrivePower(vel);
+
+            drive.update();
+
+            double heading = drive.getPoseEstimate().getHeading();
+            double deltaHeading = heading - lastHeading;
+
+            headingAccumulator += Angle.normDelta(deltaHeading);
+            lastHeading = heading;
+
+            telemetry.clearAll();
+            telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
+            telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
+            telemetry.addLine();
+            telemetry.addLine("Press Y/△ to conclude routine");
+            telemetry.update();
+
+            if (gamepad1.y)
+                tuningFinished = true;
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
+        telemetry.addLine("Effective LATERAL_DISTANCE: " +
+                (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
+
+        telemetry.update();
+
+        while (!isStopRequested()) idle();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
new file mode 100644
index 0000000..562b415
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test turning capabilities.
+ */
+@Config
+@Autonomous(group = "drive")
+public class TurnTest extends LinearOpMode {
+    public static double ANGLE = 90; // deg
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.turn(Math.toRadians(ANGLE));
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
index b9377e9..4d1da42 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
@@ -1,3 +1,131 @@
-# Teamcode
+## TeamCode Module
 
-This is where you write your code for the robot.
\ No newline at end of file
+Welcome!
+
+This module, TeamCode, is the place where you will write/paste the code for your team's
+robot controller App. This module is currently empty (a clean slate) but the
+process for adding OpModes is straightforward.
+
+## Creating your own OpModes
+
+The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
+
+Sample opmodes exist in the FtcRobotController module.
+To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
+
+Expand the following tree elements:
+ FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples
+
+### Naming of Samples
+
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.
+
+These conventions are described (in detail) in the sample_conventions.md file in this folder.
+
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:
+
+Basic:  	This is a minimally functional OpMode used to illustrate the skeleton/structure
+            of a particular style of OpMode.  These are bare bones examples.
+
+Sensor:    	This is a Sample OpMode that shows how to use a specific sensor.
+            It is not intended to drive a functioning robot, it is simply showing the minimal code
+            required to read and display the sensor values.
+
+Robot:	    This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+            It may be used to provide a common baseline driving OpMode, or
+            to demonstrate how a particular sensor or concept can be used to navigate.
+
+Concept:	This is a sample OpMode that illustrates performing a specific function or concept.
+            These may be complex, but their operation should be explained clearly in the comments,
+            or the comments should reference an external doc, guide or tutorial.
+            Each OpMode should try to only demonstrate a single concept so they are easy to
+            locate based on their name.  These OpModes may not produce a drivable robot.
+
+After the prefix, other conventions will apply:
+
+* Sensor class names are constructed as:    Sensor - Company - Type
+* Robot class names are constructed as:     Robot - Mode - Action - OpModetype
+* Concept class names are constructed as:   Concept - Topic - OpModetype
+
+Once you are familiar with the range of samples available, you can choose one to be the
+basis for your own robot.  In all cases, the desired sample(s) needs to be copied into
+your TeamCode module to be used.
+
+This is done inside Android Studio directly, using the following steps:
+
+ 1) Locate the desired sample class in the Project/Android tree.
+
+ 2) Right click on the sample class and select "Copy"
+
+ 3) Expand the  TeamCode/java folder
+
+ 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
+
+ 5) You will be prompted for a class name for the copy.
+    Choose something meaningful based on the purpose of this class.
+    Start with a capital letter, and remember that there may be more similar classes later.
+
+Once your copy has been created, you should prepare it for use on your robot.
+This is done by adjusting the OpMode's name, and enabling it to be displayed on the
+Driver Station's OpMode list.
+
+Each OpMode sample class begins with several lines of code like the ones shown below:
+
+```
+ @TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
+ @Disabled
+```
+
+The name that will appear on the driver station's "opmode list" is defined by the code:
+ ``name="Template: Linear OpMode"``
+You can change what appears between the quotes to better describe your opmode.
+The "group=" portion of the code can be used to help organize your list of OpModes.
+
+As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
+  ``@Disabled`` annotation which has been included.
+This line can simply be deleted , or commented out, to make the OpMode visible.
+
+
+
+## ADVANCED Multi-Team App management:  Cloning the TeamCode Module
+
+In some situations, you have multiple teams in your club and you want them to all share
+a common code organization, with each being able to *see* the others code but each having
+their own team module with their own code that they maintain themselves.
+
+In this situation, you might wish to clone the TeamCode module, once for each of these teams.
+Each of the clones would then appear along side each other in the Android Studio module list,
+together with the FtcRobotController module (and the original TeamCode module).
+
+Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
+prior to clicking to the green Run arrow.
+
+Warning:  This is not for the inexperienced Software developer.
+You will need to be comfortable with File manipulations and managing Android Studio Modules.
+These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
+ 
+Also.. Make a full project backup before you start this :)
+
+To clone TeamCode, do the following:
+
+Note: Some names start with "Team" and others start with "team".  This is intentional.
+
+1)  Using your operating system file management tools, copy the whole "TeamCode"
+    folder to a sibling folder with a corresponding new name, eg: "Team0417".
+
+2)  In the new Team0417 folder, delete the TeamCode.iml file.
+
+3)  the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
+    to a matching name with a lowercase 'team' eg:  "team0417".
+
+4)  In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
+         package="org.firstinspires.ftc.teamcode"
+    to be
+         package="org.firstinspires.ftc.team0417"
+
+5)  Add:    include ':Team0417' to the "/settings.gradle" file.
+    
+6)  Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SensorColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SensorColor.java
deleted file mode 100644
index 8cfc54c..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SensorColor.java
+++ /dev/null
@@ -1,222 +0,0 @@
-/* Copyright (c) 2017-2020 FIRST. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted (subject to the limitations in the disclaimer below) provided that
- * the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of FIRST nor the names of its contributors may be used to endorse or
- * promote products derived from this software without specific prior written permission.
- *
- * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
- * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-package org.firstinspires.ftc.teamcode.teleop;
-
-import android.app.Activity;
-import android.graphics.Color;
-import android.view.View;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DistanceSensor;
-import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
-import com.qualcomm.robotcore.hardware.NormalizedRGBA;
-import com.qualcomm.robotcore.hardware.SwitchableLight;
-
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-
-/**
- * This is an example LinearOpMode that shows how to use a color sensor in a generic
- * way, regardless of which particular make or model of color sensor is used. The Op Mode
- * assumes that the color sensor is configured with a name of "sensor_color".
- *
- * There will be some variation in the values measured depending on the specific sensor you are using.
- *
- * You can increase the gain (a multiplier to make the sensor report higher values) by holding down
- * the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad.
- *
- * If the color sensor has a light which is controllable from software, you can use the X button on
- * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have
- * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2.
- *
- * If the color sensor also supports short-range distance measurements (usually via an infrared
- * proximity sensor), the reported distance will be written to telemetry. As of September 2020,
- * the only color sensors that support this are the ones from REV Robotics. These infrared proximity
- * sensor measurements are only useful at very small distances, and are sensitive to ambient light
- * and surface reflectivity. You should use a different sensor if you need precise distance measurements.
- *
- * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
- * Remove or comment out the @Disabled line to add this Op Mode to the Driver Station OpMode list
- */
-@TeleOp(name = "Sensor: Color", group = "Sensor")
-public class SensorColor extends LinearOpMode {
-
-  /** The colorSensor field will contain a reference to our color sensor hardware object */
-  NormalizedColorSensor colorSensor;
-
-  /** The relativeLayout field is used to aid in providing interesting visual feedback
-   * in this sample application; you probably *don't* need this when you use a color sensor on your
-   * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */
-  View relativeLayout;
-
-  /**
-   * The runOpMode() method is the root of this Op Mode, as it is in all LinearOpModes.
-   * Our implementation here, though is a bit unusual: we've decided to put all the actual work
-   * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is
-   * that in this sample we're changing the background color of the robot controller screen as the
-   * Op Mode runs, and we want to be able to *guarantee* that we restore it to something reasonable
-   * and palatable when the Op Mode ends. The simplest way to do that is to use a try...finally
-   * block around the main, core logic, and an easy way to make that all clear was to separate
-   * the former from the latter in separate methods.
-   */
-  @Override public void runOpMode() {
-
-    // Get a reference to the RelativeLayout so we can later change the background
-    // color of the Robot Controller app to match the hue detected by the RGB sensor.
-    int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
-    relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);
-
-    try {
-      runSample(); // actually execute the sample
-    } finally {
-      // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off
-      // as pure white, but it's too much work to dig out what actually was used, and this is good
-      // enough to at least make the screen reasonable again.
-      // Set the panel back to the default color
-      relativeLayout.post(new Runnable() {
-        public void run() {
-          relativeLayout.setBackgroundColor(Color.WHITE);
-        }
-      });
-      }
-  }
-
-  protected void runSample() {
-    // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the
-    // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3)
-    // can give very low values (depending on the lighting conditions), which only use a small part
-    // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions,
-    // you should use a smaller gain than in dark conditions. If your gain is too high, all of the
-    // colors will report at or near 1, and you won't be able to determine what color you are
-    // actually looking at. For this reason, it's better to err on the side of a lower gain
-    // (but always greater than  or equal to 1).
-    float gain = 2;
-
-    // Once per loop, we will update this hsvValues array. The first element (0) will contain the
-    // hue, the second element (1) will contain the saturation, and the third element (2) will
-    // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
-    // for an explanation of HSV color.
-    final float[] hsvValues = new float[3];
-
-    // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current
-    // state of the X button on the gamepad
-    boolean xButtonPreviouslyPressed = false;
-    boolean xButtonCurrentlyPressed = false;
-
-    // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over
-    // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while
-    // the values you get from ColorSensor are dependent on the specific sensor you're using.
-    colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
-
-    // If possible, turn the light on in the beginning (it might already be on anyway,
-    // we just make sure it is if we can).
-    if (colorSensor instanceof SwitchableLight) {
-      ((SwitchableLight)colorSensor).enableLight(true);
-    }
-
-    // Wait for the start button to be pressed.
-    waitForStart();
-
-    // Loop until we are asked to stop
-    while (opModeIsActive()) {
-      // Explain basic gain information via telemetry
-      telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n");
-      telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n");
-
-      // Update the gain value if either of the A or B gamepad buttons is being held
-      if (gamepad1.a) {
-        // Only increase the gain by a small amount, since this loop will occur multiple times per second.
-        gain += 0.005;
-      } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful.
-        gain -= 0.005;
-      }
-
-      // Show the gain value via telemetry
-      telemetry.addData("Gain", gain);
-
-      // Tell the sensor our desired gain value (normally you would do this during initialization,
-      // not during the loop)
-      colorSensor.setGain(gain);
-
-      // Check the status of the X button on the gamepad
-      xButtonCurrentlyPressed = gamepad1.x;
-
-      // If the button state is different than what it was, then act
-      if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) {
-        // If the button is (now) down, then toggle the light
-        if (xButtonCurrentlyPressed) {
-          if (colorSensor instanceof SwitchableLight) {
-            SwitchableLight light = (SwitchableLight)colorSensor;
-            light.enableLight(!light.isLightOn());
-          }
-        }
-      }
-      xButtonPreviouslyPressed = xButtonCurrentlyPressed;
-
-      // Get the normalized colors from the sensor
-      NormalizedRGBA colors = colorSensor.getNormalizedColors();
-
-      /* Use telemetry to display feedback on the driver station. We show the red, green, and blue
-       * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent
-       * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
-       * for an explanation of HSV color. */
-
-      // Update the hsvValues array by passing it to Color.colorToHSV()
-      Color.colorToHSV(colors.toColor(), hsvValues);
-
-      telemetry.addLine()
-              .addData("Red", "%.3f", colors.red)
-              .addData("Green", "%.3f", colors.green)
-              .addData("Blue", "%.3f", colors.blue);
-      telemetry.addLine()
-              .addData("Hue", "%.3f", hsvValues[0])
-              .addData("Saturation", "%.3f", hsvValues[1])
-              .addData("Value", "%.3f", hsvValues[2]);
-      telemetry.addData("Alpha", "%.3f", colors.alpha);
-
-      /* If this color sensor also has a distance sensor, display the measured distance.
-       * Note that the reported distance is only useful at very close range, and is impacted by
-       * ambient light and surface reflectivity. */
-      if (colorSensor instanceof DistanceSensor) {
-        telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM));
-      }
-
-      telemetry.update();
-
-      // Change the Robot Controller's background color to match the color detected by the color sensor.
-      relativeLayout.post(new Runnable() {
-        public void run() {
-          relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues));
-        }
-      });
-    }
-  }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleop.java
deleted file mode 100644
index e5f27a3..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleop.java
+++ /dev/null
@@ -1,27 +0,0 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.RobotOpMode;
-import org.firstinspires.ftc.teamcode.subsystem.LinearSlide;
-import org.firstinspires.ftc.teamcode.subsystem.StrafeDrive;
-
-@TeleOp(name = "2 Player Teleop")
-public class TwoPlayerTeleop extends RobotOpMode {
-    private final StrafeDrive drive = new StrafeDrive(this);
-    private final LinearSlide slide = new LinearSlide(this);
-
-    @Override
-    public void runOpMode() {
-        addSubsystems(drive, slide);
-
-        waitForStart();
-
-        while (opModeIsActive()) {
-            drive.updateByTwoGamepads();
-            slide.readGamepad();
-            slide.updateTelemetry();
-            telemetry.update();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java
new file mode 100644
index 0000000..081d695
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+
+public class EmptySequenceException extends RuntimeException { }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java
new file mode 100644
index 0000000..29032e6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java
@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
+
+import java.util.Collections;
+import java.util.List;
+
+public class TrajectorySequence {
+    private final List<SequenceSegment> sequenceList;
+
+    public TrajectorySequence(List<SequenceSegment> sequenceList) {
+        if (sequenceList.size() == 0) throw new EmptySequenceException();
+
+        this.sequenceList = Collections.unmodifiableList(sequenceList);
+    }
+
+    public Pose2d start() {
+        return sequenceList.get(0).getStartPose();
+    }
+
+    public Pose2d end() {
+        return sequenceList.get(sequenceList.size() - 1).getEndPose();
+    }
+
+    public double duration() {
+        double total = 0.0;
+
+        for (SequenceSegment segment : sequenceList) {
+            total += segment.getDuration();
+        }
+
+        return total;
+    }
+
+    public SequenceSegment get(int i) {
+        return sequenceList.get(i);
+    }
+
+    public int size() {
+        return sequenceList.size();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java
new file mode 100644
index 0000000..5da64fd
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java
@@ -0,0 +1,711 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
+import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
+import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
+import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
+import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
+import com.acmerobotics.roadrunner.trajectory.TimeProducer;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class TrajectorySequenceBuilder {
+    private final double resolution = 0.25;
+
+    private final TrajectoryVelocityConstraint baseVelConstraint;
+    private final TrajectoryAccelerationConstraint baseAccelConstraint;
+
+    private TrajectoryVelocityConstraint currentVelConstraint;
+    private TrajectoryAccelerationConstraint currentAccelConstraint;
+
+    private final double baseTurnConstraintMaxAngVel;
+    private final double baseTurnConstraintMaxAngAccel;
+
+    private double currentTurnConstraintMaxAngVel;
+    private double currentTurnConstraintMaxAngAccel;
+
+    private final List<SequenceSegment> sequenceSegments;
+
+    private final List<TemporalMarker> temporalMarkers;
+    private final List<DisplacementMarker> displacementMarkers;
+    private final List<SpatialMarker> spatialMarkers;
+
+    private Pose2d lastPose;
+
+    private double tangentOffset;
+
+    private boolean setAbsoluteTangent;
+    private double absoluteTangent;
+
+    private TrajectoryBuilder currentTrajectoryBuilder;
+
+    private double currentDuration;
+    private double currentDisplacement;
+
+    private double lastDurationTraj;
+    private double lastDisplacementTraj;
+
+    public TrajectorySequenceBuilder(
+            Pose2d startPose,
+            Double startTangent,
+            TrajectoryVelocityConstraint baseVelConstraint,
+            TrajectoryAccelerationConstraint baseAccelConstraint,
+            double baseTurnConstraintMaxAngVel,
+            double baseTurnConstraintMaxAngAccel
+    ) {
+        this.baseVelConstraint = baseVelConstraint;
+        this.baseAccelConstraint = baseAccelConstraint;
+
+        this.currentVelConstraint = baseVelConstraint;
+        this.currentAccelConstraint = baseAccelConstraint;
+
+        this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+        this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+        this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+        this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+        sequenceSegments = new ArrayList<>();
+
+        temporalMarkers = new ArrayList<>();
+        displacementMarkers = new ArrayList<>();
+        spatialMarkers = new ArrayList<>();
+
+        lastPose = startPose;
+
+        tangentOffset = 0.0;
+
+        setAbsoluteTangent = (startTangent != null);
+        absoluteTangent = startTangent != null ? startTangent : 0.0;
+
+        currentTrajectoryBuilder = null;
+
+        currentDuration = 0.0;
+        currentDisplacement = 0.0;
+
+        lastDurationTraj = 0.0;
+        lastDisplacementTraj = 0.0;
+    }
+
+    public TrajectorySequenceBuilder(
+            Pose2d startPose,
+            TrajectoryVelocityConstraint baseVelConstraint,
+            TrajectoryAccelerationConstraint baseAccelConstraint,
+            double baseTurnConstraintMaxAngVel,
+            double baseTurnConstraintMaxAngAccel
+    ) {
+        this(
+                startPose, null,
+                baseVelConstraint, baseAccelConstraint,
+                baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
+        );
+    }
+
+    public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
+        return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineTo(
+            Vector2d endPosition,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
+        return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToConstantHeading(
+            Vector2d endPosition,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
+        return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToLinearHeading(
+            Pose2d endPose,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
+        return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToSplineHeading(
+            Pose2d endPose,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
+        return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeTo(
+            Vector2d endPosition,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder forward(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder forward(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder back(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder back(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeLeft(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeLeft(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeRight(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeRight(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineTo(
+            Vector2d endPosition,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToConstantHeading(
+            Vector2d endPosition,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToLinearHeading(
+            Pose2d endPose,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToSplineHeading(
+            Pose2d endPose,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
+    }
+
+    private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
+        if (currentTrajectoryBuilder == null) newPath();
+
+        try {
+            callback.run();
+        } catch (PathContinuityViolationException e) {
+            newPath();
+            callback.run();
+        }
+
+        Trajectory builtTraj = currentTrajectoryBuilder.build();
+
+        double durationDifference = builtTraj.duration() - lastDurationTraj;
+        double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
+
+        lastPose = builtTraj.end();
+        currentDuration += durationDifference;
+        currentDisplacement += displacementDifference;
+
+        lastDurationTraj = builtTraj.duration();
+        lastDisplacementTraj = builtTraj.getPath().length();
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setTangent(double tangent) {
+        setAbsoluteTangent = true;
+        absoluteTangent = tangent;
+
+        pushPath();
+
+        return this;
+    }
+
+    private TrajectorySequenceBuilder setTangentOffset(double offset) {
+        setAbsoluteTangent = false;
+
+        this.tangentOffset = offset;
+        this.pushPath();
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setReversed(boolean reversed) {
+        return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
+    }
+
+    public TrajectorySequenceBuilder setConstraints(
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        this.currentVelConstraint = velConstraint;
+        this.currentAccelConstraint = accelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetConstraints() {
+        this.currentVelConstraint = this.baseVelConstraint;
+        this.currentAccelConstraint = this.baseAccelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
+        this.currentVelConstraint = velConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetVelConstraint() {
+        this.currentVelConstraint = this.baseVelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
+        this.currentAccelConstraint = accelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetAccelConstraint() {
+        this.currentAccelConstraint = this.baseAccelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
+        this.currentTurnConstraintMaxAngVel = maxAngVel;
+        this.currentTurnConstraintMaxAngAccel = maxAngAccel;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetTurnConstraint() {
+        this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+        this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
+        return this.addTemporalMarker(currentDuration, callback);
+    }
+
+    public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
+        return this.addTemporalMarker(currentDuration + offset, callback);
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
+        return this.addTemporalMarker(0.0, time, callback);
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
+        return this.addTemporalMarker(time -> scale * time + offset, callback);
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
+        this.temporalMarkers.add(new TemporalMarker(time, callback));
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
+        this.spatialMarkers.add(new SpatialMarker(point, callback));
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
+        return this.addDisplacementMarker(currentDisplacement, callback);
+    }
+
+    public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
+        return this.addDisplacementMarker(currentDisplacement + offset, callback);
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
+        return this.addDisplacementMarker(0.0, displacement, callback);
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
+        return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
+        displacementMarkers.add(new DisplacementMarker(displacement, callback));
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder turn(double angle) {
+        return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
+    }
+
+    public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
+        pushPath();
+
+        MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
+                new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
+                new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
+                maxAngVel,
+                maxAngAccel
+        );
+
+        sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
+
+        lastPose = new Pose2d(
+                lastPose.getX(), lastPose.getY(),
+                Angle.norm(lastPose.getHeading() + angle)
+        );
+
+        currentDuration += turnProfile.duration();
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder waitSeconds(double seconds) {
+        pushPath();
+        sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
+
+        currentDuration += seconds;
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
+        pushPath();
+
+        sequenceSegments.add(new TrajectorySegment(trajectory));
+        return this;
+    }
+
+    private void pushPath() {
+        if (currentTrajectoryBuilder != null) {
+            Trajectory builtTraj = currentTrajectoryBuilder.build();
+            sequenceSegments.add(new TrajectorySegment(builtTraj));
+        }
+
+        currentTrajectoryBuilder = null;
+    }
+
+    private void newPath() {
+        if (currentTrajectoryBuilder != null)
+            pushPath();
+
+        lastDurationTraj = 0.0;
+        lastDisplacementTraj = 0.0;
+
+        double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
+
+        currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
+    }
+
+    public TrajectorySequence build() {
+        pushPath();
+
+        List<TrajectoryMarker> globalMarkers = convertMarkersToGlobal(
+                sequenceSegments,
+                temporalMarkers, displacementMarkers, spatialMarkers
+        );
+
+        return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
+    }
+
+    private List<TrajectoryMarker> convertMarkersToGlobal(
+            List<SequenceSegment> sequenceSegments,
+            List<TemporalMarker> temporalMarkers,
+            List<DisplacementMarker> displacementMarkers,
+            List<SpatialMarker> spatialMarkers
+    ) {
+        ArrayList<TrajectoryMarker> trajectoryMarkers = new ArrayList<>();
+
+        // Convert temporal markers
+        for (TemporalMarker marker : temporalMarkers) {
+            trajectoryMarkers.add(
+                    new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
+            );
+        }
+
+        // Convert displacement markers
+        for (DisplacementMarker marker : displacementMarkers) {
+            double time = displacementToTime(
+                    sequenceSegments,
+                    marker.getProducer().produce(currentDisplacement)
+            );
+
+            trajectoryMarkers.add(
+                    new TrajectoryMarker(
+                            time,
+                            marker.getCallback()
+                    )
+            );
+        }
+
+        // Convert spatial markers
+        for (SpatialMarker marker : spatialMarkers) {
+            trajectoryMarkers.add(
+                    new TrajectoryMarker(
+                            pointToTime(sequenceSegments, marker.getPoint()),
+                            marker.getCallback()
+                    )
+            );
+        }
+
+        return trajectoryMarkers;
+    }
+
+    private List<SequenceSegment> projectGlobalMarkersToLocalSegments(List<TrajectoryMarker> markers, List<SequenceSegment> sequenceSegments) {
+        if (sequenceSegments.isEmpty()) return Collections.emptyList();
+
+        double totalSequenceDuration = 0;
+        for (SequenceSegment segment : sequenceSegments) {
+            totalSequenceDuration += segment.getDuration();
+        }
+
+        for (TrajectoryMarker marker : markers) {
+            SequenceSegment segment = null;
+            int segmentIndex = 0;
+            double segmentOffsetTime = 0;
+
+            double currentTime = 0;
+            for (int i = 0; i < sequenceSegments.size(); i++) {
+                SequenceSegment seg = sequenceSegments.get(i);
+
+                double markerTime = Math.min(marker.getTime(), totalSequenceDuration);
+
+                if (currentTime + seg.getDuration() >= markerTime) {
+                    segment = seg;
+                    segmentIndex = i;
+                    segmentOffsetTime = markerTime - currentTime;
+
+                    break;
+                } else {
+                    currentTime += seg.getDuration();
+                }
+            }
+
+            SequenceSegment newSegment = null;
+
+            if (segment instanceof WaitSegment) {
+                List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
+
+                newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
+                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+                WaitSegment thisSegment = (WaitSegment) segment;
+                newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers);
+            } else if (segment instanceof TurnSegment) {
+                List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
+
+                newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
+                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+                TurnSegment thisSegment = (TurnSegment) segment;
+                newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers);
+            } else if (segment instanceof TrajectorySegment) {
+                TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+                List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
+                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+                newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers));
+            }
+
+            sequenceSegments.set(segmentIndex, newSegment);
+        }
+
+        return sequenceSegments;
+    }
+
+    // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
+    // note: this assumes that the profile position is monotonic increasing
+    private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
+        double tLo = 0.0;
+        double tHi = profile.duration();
+        while (!(Math.abs(tLo - tHi) < 1e-6)) {
+            double tMid = 0.5 * (tLo + tHi);
+            if (profile.get(tMid).getX() > s) {
+                tHi = tMid;
+            } else {
+                tLo = tMid;
+            }
+        }
+        return 0.5 * (tLo + tHi);
+    }
+
+    private Double displacementToTime(List<SequenceSegment> sequenceSegments, double s) {
+        double currentTime = 0.0;
+        double currentDisplacement = 0.0;
+
+        for (SequenceSegment segment : sequenceSegments) {
+            if (segment instanceof TrajectorySegment) {
+                TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+                double segmentLength = thisSegment.getTrajectory().getPath().length();
+
+                if (currentDisplacement + segmentLength > s) {
+                    double target = s - currentDisplacement;
+                    double timeInSegment = motionProfileDisplacementToTime(
+                            thisSegment.getTrajectory().getProfile(),
+                            target
+                    );
+
+                    return currentTime + timeInSegment;
+                } else {
+                    currentDisplacement += segmentLength;
+                    currentTime += thisSegment.getTrajectory().duration();
+                }
+            } else {
+                currentTime += segment.getDuration();
+            }
+        }
+
+        return 0.0;
+    }
+
+    private Double pointToTime(List<SequenceSegment> sequenceSegments, Vector2d point) {
+        class ComparingPoints {
+            private final double distanceToPoint;
+            private final double totalDisplacement;
+            private final double thisPathDisplacement;
+
+            public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
+                this.distanceToPoint = distanceToPoint;
+                this.totalDisplacement = totalDisplacement;
+                this.thisPathDisplacement = thisPathDisplacement;
+            }
+        }
+
+        List<ComparingPoints> projectedPoints = new ArrayList<>();
+
+        for (SequenceSegment segment : sequenceSegments) {
+            if (segment instanceof TrajectorySegment) {
+                TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+                double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
+                Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
+                double distanceToPoint = point.minus(projectedPoint).norm();
+
+                double totalDisplacement = 0.0;
+
+                for (ComparingPoints comparingPoint : projectedPoints) {
+                    totalDisplacement += comparingPoint.totalDisplacement;
+                }
+
+                totalDisplacement += displacement;
+
+                projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
+            }
+        }
+
+        ComparingPoints closestPoint = null;
+
+        for (ComparingPoints comparingPoint : projectedPoints) {
+            if (closestPoint == null) {
+                closestPoint = comparingPoint;
+                continue;
+            }
+
+            if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
+                closestPoint = comparingPoint;
+        }
+
+        return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
+    }
+
+    private interface AddPathCallback {
+        void run();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java
new file mode 100644
index 0000000..439c005
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java
@@ -0,0 +1,279 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.NanoClock;
+
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
+import org.firstinspires.ftc.teamcode.util.DashboardUtil;
+import org.firstinspires.ftc.teamcode.util.LogFiles;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public class TrajectorySequenceRunner {
+    public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
+    public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
+    public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
+
+    public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
+    public static String COLOR_ACTIVE_TURN = "#7c4dff";
+    public static String COLOR_ACTIVE_WAIT = "#dd2c00";
+
+    public static int POSE_HISTORY_LIMIT = 100;
+
+    private final TrajectoryFollower follower;
+
+    private final PIDFController turnController;
+
+    private final NanoClock clock;
+
+    private TrajectorySequence currentTrajectorySequence;
+    private double currentSegmentStartTime;
+    private int currentSegmentIndex;
+    private int lastSegmentIndex;
+
+    private Pose2d lastPoseError = new Pose2d();
+
+    List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
+
+    private final FtcDashboard dashboard;
+    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
+
+    public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) {
+        this.follower = follower;
+
+        turnController = new PIDFController(headingPIDCoefficients);
+        turnController.setInputBounds(0, 2 * Math.PI);
+
+        clock = NanoClock.system();
+
+        dashboard = FtcDashboard.getInstance();
+        dashboard.setTelemetryTransmissionInterval(25);
+    }
+
+    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+        currentTrajectorySequence = trajectorySequence;
+        currentSegmentStartTime = clock.seconds();
+        currentSegmentIndex = 0;
+        lastSegmentIndex = -1;
+    }
+
+    public @Nullable
+    DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
+        Pose2d targetPose = null;
+        DriveSignal driveSignal = null;
+
+        TelemetryPacket packet = new TelemetryPacket();
+        Canvas fieldOverlay = packet.fieldOverlay();
+
+        SequenceSegment currentSegment = null;
+
+        if (currentTrajectorySequence != null) {
+            if (currentSegmentIndex >= currentTrajectorySequence.size()) {
+                for (TrajectoryMarker marker : remainingMarkers) {
+                    marker.getCallback().onMarkerReached();
+                }
+
+                remainingMarkers.clear();
+
+                currentTrajectorySequence = null;
+            }
+
+            if (currentTrajectorySequence == null)
+                return new DriveSignal();
+
+            double now = clock.seconds();
+            boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
+
+            currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
+
+            if (isNewTransition) {
+                currentSegmentStartTime = now;
+                lastSegmentIndex = currentSegmentIndex;
+
+                for (TrajectoryMarker marker : remainingMarkers) {
+                    marker.getCallback().onMarkerReached();
+                }
+
+                remainingMarkers.clear();
+
+                remainingMarkers.addAll(currentSegment.getMarkers());
+                Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
+            }
+
+            double deltaTime = now - currentSegmentStartTime;
+
+            if (currentSegment instanceof TrajectorySegment) {
+                Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+                if (isNewTransition)
+                    follower.followTrajectory(currentTrajectory);
+
+                if (!follower.isFollowing()) {
+                    currentSegmentIndex++;
+
+                    driveSignal = new DriveSignal();
+                } else {
+                    driveSignal = follower.update(poseEstimate, poseVelocity);
+                    lastPoseError = follower.getLastError();
+                }
+
+                targetPose = currentTrajectory.get(deltaTime);
+            } else if (currentSegment instanceof TurnSegment) {
+                MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
+
+                turnController.setTargetPosition(targetState.getX());
+
+                double correction = turnController.update(poseEstimate.getHeading());
+
+                double targetOmega = targetState.getV();
+                double targetAlpha = targetState.getA();
+
+                lastPoseError = new Pose2d(0, 0, turnController.getLastError());
+
+                Pose2d startPose = currentSegment.getStartPose();
+                targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
+
+                driveSignal = new DriveSignal(
+                        new Pose2d(0, 0, targetOmega + correction),
+                        new Pose2d(0, 0, targetAlpha)
+                );
+
+                if (deltaTime >= currentSegment.getDuration()) {
+                    currentSegmentIndex++;
+                    driveSignal = new DriveSignal();
+                }
+            } else if (currentSegment instanceof WaitSegment) {
+                lastPoseError = new Pose2d();
+
+                targetPose = currentSegment.getStartPose();
+                driveSignal = new DriveSignal();
+
+                if (deltaTime >= currentSegment.getDuration()) {
+                    currentSegmentIndex++;
+                }
+            }
+
+            while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
+                remainingMarkers.get(0).getCallback().onMarkerReached();
+                remainingMarkers.remove(0);
+            }
+        }
+
+        poseHistory.add(poseEstimate);
+
+        if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
+            poseHistory.removeFirst();
+        }
+
+        if (targetPose != null) {
+            LogFiles.recordPose(poseEstimate);
+            LogFiles.recordTargetPose(targetPose);
+        }
+
+        packet.put("x", poseEstimate.getX());
+        packet.put("y", poseEstimate.getY());
+        packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
+
+        packet.put("xError", getLastPoseError().getX());
+        packet.put("yError", getLastPoseError().getY());
+        packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
+
+        draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
+
+        dashboard.sendTelemetryPacket(packet);
+
+        return driveSignal;
+    }
+
+    private void draw(
+            Canvas fieldOverlay,
+            TrajectorySequence sequence, SequenceSegment currentSegment,
+            Pose2d targetPose, Pose2d poseEstimate
+    ) {
+        if (sequence != null) {
+            for (int i = 0; i < sequence.size(); i++) {
+                SequenceSegment segment = sequence.get(i);
+
+                if (segment instanceof TrajectorySegment) {
+                    fieldOverlay.setStrokeWidth(1);
+                    fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
+
+                    DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
+                } else if (segment instanceof TurnSegment) {
+                    Pose2d pose = segment.getStartPose();
+
+                    fieldOverlay.setFill(COLOR_INACTIVE_TURN);
+                    fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
+                } else if (segment instanceof WaitSegment) {
+                    Pose2d pose = segment.getStartPose();
+
+                    fieldOverlay.setStrokeWidth(1);
+                    fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
+                    fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+                }
+            }
+        }
+
+        if (currentSegment != null) {
+            if (currentSegment instanceof TrajectorySegment) {
+                Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+                fieldOverlay.setStrokeWidth(1);
+                fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
+
+                DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
+            } else if (currentSegment instanceof TurnSegment) {
+                Pose2d pose = currentSegment.getStartPose();
+
+                fieldOverlay.setFill(COLOR_ACTIVE_TURN);
+                fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
+            } else if (currentSegment instanceof WaitSegment) {
+                Pose2d pose = currentSegment.getStartPose();
+
+                fieldOverlay.setStrokeWidth(1);
+                fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
+                fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+            }
+        }
+
+        if (targetPose != null) {
+            fieldOverlay.setStrokeWidth(1);
+            fieldOverlay.setStroke("#4CAF50");
+            DashboardUtil.drawRobot(fieldOverlay, targetPose);
+        }
+
+        fieldOverlay.setStroke("#3F51B5");
+        DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
+
+        fieldOverlay.setStroke("#3F51B5");
+        DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
+    }
+
+    public Pose2d getLastPoseError() {
+        return lastPoseError;
+    }
+
+    public boolean isBusy() {
+        return currentTrajectorySequence != null;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java
new file mode 100644
index 0000000..e2eb4ca
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public abstract class SequenceSegment {
+    private final double duration;
+    private final Pose2d startPose;
+    private final Pose2d endPose;
+    private final List<TrajectoryMarker> markers;
+
+    protected SequenceSegment(
+            double duration,
+            Pose2d startPose, Pose2d endPose,
+            List<TrajectoryMarker> markers
+    ) {
+        this.duration = duration;
+        this.startPose = startPose;
+        this.endPose = endPose;
+        this.markers = markers;
+    }
+
+    public double getDuration() {
+        return this.duration;
+    }
+
+    public Pose2d getStartPose() {
+        return startPose;
+    }
+
+    public Pose2d getEndPose() {
+        return endPose;
+    }
+
+    public List<TrajectoryMarker> getMarkers() {
+        return markers;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java
new file mode 100644
index 0000000..2e735d4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+
+import java.util.Collections;
+
+public final class TrajectorySegment extends SequenceSegment {
+    private final Trajectory trajectory;
+
+    public TrajectorySegment(Trajectory trajectory) {
+        // Note: Markers are already stored in the `Trajectory` itself.
+        // This class should not hold any markers
+        super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
+        this.trajectory = trajectory;
+    }
+
+    public Trajectory getTrajectory() {
+        return this.trajectory;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java
new file mode 100644
index 0000000..11995db
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import java.util.List;
+
+public final class TurnSegment extends SequenceSegment {
+    private final double totalRotation;
+    private final MotionProfile motionProfile;
+
+    public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List<TrajectoryMarker> markers) {
+        super(
+                motionProfile.duration(),
+                startPose,
+                new Pose2d(
+                        startPose.getX(), startPose.getY(),
+                        Angle.norm(startPose.getHeading() + totalRotation)
+                ),
+                markers
+        );
+
+        this.totalRotation = totalRotation;
+        this.motionProfile = motionProfile;
+    }
+
+    public final double getTotalRotation() {
+        return this.totalRotation;
+    }
+
+    public final MotionProfile getMotionProfile() {
+        return this.motionProfile;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java
new file mode 100644
index 0000000..62d2ba4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public final class WaitSegment extends SequenceSegment {
+    public WaitSegment(Pose2d pose, double seconds, List<TrajectoryMarker> markers) {
+        super(seconds, pose, pose, markers);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
new file mode 100644
index 0000000..1b34a95
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.IOException;
+import java.io.InputStream;
+
+/**
+ * Set of utilities for loading trajectories from assets (the plugin save location).
+ */
+public class AssetsTrajectoryManager {
+
+    /**
+     * Loads the group config.
+     */
+    public static @Nullable
+    TrajectoryGroupConfig loadGroupConfig() {
+        try {
+            InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+                    "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
+            return TrajectoryConfigManager.loadGroupConfig(inputStream);
+        } catch (IOException e) {
+            return null;
+        }
+    }
+
+    /**
+     * Loads a trajectory config with the given name.
+     */
+    public static @Nullable TrajectoryConfig loadConfig(String name) {
+        try {
+            InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+                    "trajectory/" + name + ".yaml");
+            return TrajectoryConfigManager.loadConfig(inputStream);
+        } catch (IOException e) {
+            return null;
+        }
+    }
+
+    /**
+     * Loads a trajectory builder with the given name.
+     */
+    public static @Nullable TrajectoryBuilder loadBuilder(String name) {
+        TrajectoryGroupConfig groupConfig = loadGroupConfig();
+        TrajectoryConfig config = loadConfig(name);
+        if (groupConfig == null || config == null) {
+            return null;
+        }
+        return config.toTrajectoryBuilder(groupConfig);
+    }
+
+    /**
+     * Loads a trajectory with the given name.
+     */
+    public static @Nullable Trajectory load(String name) {
+        TrajectoryBuilder builder = loadBuilder(name);
+        if (builder == null) {
+            return null;
+        }
+        return builder.build();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java
new file mode 100644
index 0000000..42271d6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.util;
+
+/**
+ * IMU axes signs in the order XYZ (after remapping).
+ */
+public enum AxesSigns {
+    PPP(0b000),
+    PPN(0b001),
+    PNP(0b010),
+    PNN(0b011),
+    NPP(0b100),
+    NPN(0b101),
+    NNP(0b110),
+    NNN(0b111);
+
+    public final int bVal;
+
+    AxesSigns(int bVal) {
+        this.bVal = bVal;
+    }
+
+    public static AxesSigns fromBinaryValue(int bVal) {
+        int maskedVal = bVal & 0x07;
+        switch (maskedVal) {
+            case 0b000:
+                return AxesSigns.PPP;
+            case 0b001:
+                return AxesSigns.PPN;
+            case 0b010:
+                return AxesSigns.PNP;
+            case 0b011:
+                return AxesSigns.PNN;
+            case 0b100:
+                return AxesSigns.NPP;
+            case 0b101:
+                return AxesSigns.NPN;
+            case 0b110:
+                return AxesSigns.NNP;
+            case 0b111:
+                return AxesSigns.NNN;
+            default:
+                throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java
new file mode 100644
index 0000000..5b8279b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.util;
+
+/**
+ * A direction for an axis to be remapped to
+ */
+public enum AxisDirection {
+    POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java
new file mode 100644
index 0000000..ce11d8d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java
@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.Path;
+
+import java.util.List;
+
+/**
+ * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
+ */
+public class DashboardUtil {
+    private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
+    private static final double ROBOT_RADIUS = 9; // in
+
+
+    public static void drawPoseHistory(Canvas canvas, List<Pose2d> poseHistory) {
+        double[] xPoints = new double[poseHistory.size()];
+        double[] yPoints = new double[poseHistory.size()];
+        for (int i = 0; i < poseHistory.size(); i++) {
+            Pose2d pose = poseHistory.get(i);
+            xPoints[i] = pose.getX();
+            yPoints[i] = pose.getY();
+        }
+        canvas.strokePolyline(xPoints, yPoints);
+    }
+
+    public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
+        int samples = (int) Math.ceil(path.length() / resolution);
+        double[] xPoints = new double[samples];
+        double[] yPoints = new double[samples];
+        double dx = path.length() / (samples - 1);
+        for (int i = 0; i < samples; i++) {
+            double displacement = i * dx;
+            Pose2d pose = path.get(displacement);
+            xPoints[i] = pose.getX();
+            yPoints[i] = pose.getY();
+        }
+        canvas.strokePolyline(xPoints, yPoints);
+    }
+
+    public static void drawSampledPath(Canvas canvas, Path path) {
+        drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
+    }
+
+    public static void drawRobot(Canvas canvas, Pose2d pose) {
+        canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
+        Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
+        double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
+        double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
+        canvas.strokeLine(x1, y1, x2, y2);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java
new file mode 100644
index 0000000..f724c52
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java
@@ -0,0 +1,125 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+/**
+ * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
+ * slot's motor direction
+ */
+public class Encoder {
+    private final static int CPS_STEP = 0x10000;
+
+    private static double inverseOverflow(double input, double estimate) {
+        // convert to uint16
+        int real = (int) input & 0xffff;
+        // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
+        // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
+        real += ((real % 20) / 4) * CPS_STEP;
+        // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
+        real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
+        return real;
+    }
+
+    public enum Direction {
+        FORWARD(1),
+        REVERSE(-1);
+
+        private int multiplier;
+
+        Direction(int multiplier) {
+            this.multiplier = multiplier;
+        }
+
+        public int getMultiplier() {
+            return multiplier;
+        }
+    }
+
+    private DcMotorEx motor;
+    private NanoClock clock;
+
+    private Direction direction;
+
+    private int lastPosition;
+    private int velocityEstimateIdx;
+    private double[] velocityEstimates;
+    private double lastUpdateTime;
+
+    public Encoder(DcMotorEx motor, NanoClock clock) {
+        this.motor = motor;
+        this.clock = clock;
+
+        this.direction = Direction.FORWARD;
+
+        this.lastPosition = 0;
+        this.velocityEstimates = new double[3];
+        this.lastUpdateTime = clock.seconds();
+    }
+
+    public Encoder(DcMotorEx motor) {
+        this(motor, NanoClock.system());
+    }
+
+    public Direction getDirection() {
+        return direction;
+    }
+
+    private int getMultiplier() {
+        return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
+    }
+
+    /**
+     * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
+     * @param direction either reverse or forward depending on if encoder counts should be negated
+     */
+    public void setDirection(Direction direction) {
+        this.direction = direction;
+    }
+
+    /**
+     * Gets the position from the underlying motor and adjusts for the set direction.
+     * Additionally, this method updates the velocity estimates used for compensated velocity
+     *
+     * @return encoder position
+     */
+    public int getCurrentPosition() {
+        int multiplier = getMultiplier();
+        int currentPosition = motor.getCurrentPosition() * multiplier;
+        if (currentPosition != lastPosition) {
+            double currentTime = clock.seconds();
+            double dt = currentTime - lastUpdateTime;
+            velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
+            velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
+            lastPosition = currentPosition;
+            lastUpdateTime = currentTime;
+        }
+        return currentPosition;
+    }
+
+    /**
+     * Gets the velocity directly from the underlying motor and compensates for the direction
+     * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
+     *
+     * @return raw velocity
+     */
+    public double getRawVelocity() {
+        int multiplier = getMultiplier();
+        return motor.getVelocity() * multiplier;
+    }
+
+    /**
+     * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
+     * that are lost in overflow due to velocity being transmitted as 16 bits.
+     * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
+     *
+     * @return corrected velocity
+     */
+    public double getCorrectedVelocity() {
+        double median = velocityEstimates[0] > velocityEstimates[1]
+                ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
+                : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
+        return inverseOverflow(getRawVelocity(), median);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
new file mode 100644
index 0000000..66e5e9d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
@@ -0,0 +1,226 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import android.annotation.SuppressLint;
+import android.content.Context;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.fasterxml.jackson.core.JsonFactory;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.ObjectWriter;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.WebHandlerManager;
+
+import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleTankDrive;
+import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.text.DateFormat;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Date;
+import java.util.List;
+import java.util.Objects;
+
+import fi.iki.elonen.NanoHTTPD;
+
+public final class LogFiles {
+    private static final File ROOT =
+            new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/");
+
+    public static LogFile log = new LogFile("uninitialized");
+
+    public static class LogFile {
+        public String version = "quickstart1 v0";
+
+        public String opModeName;
+        public long msInit = System.currentTimeMillis();
+        public long nsInit = System.nanoTime();
+        public long nsStart, nsStop;
+
+        public double ticksPerRev = DriveConstants.TICKS_PER_REV;
+        public double maxRpm = DriveConstants.MAX_RPM;
+        public boolean runUsingEncoder = DriveConstants.RUN_USING_ENCODER;
+        public double motorP = DriveConstants.MOTOR_VELO_PID.p;
+        public double motorI = DriveConstants.MOTOR_VELO_PID.i;
+        public double motorD = DriveConstants.MOTOR_VELO_PID.d;
+        public double motorF = DriveConstants.MOTOR_VELO_PID.f;
+        public double wheelRadius = DriveConstants.WHEEL_RADIUS;
+        public double gearRatio = DriveConstants.GEAR_RATIO;
+        public double trackWidth = DriveConstants.TRACK_WIDTH;
+        public double kV = DriveConstants.kV;
+        public double kA = DriveConstants.kA;
+        public double kStatic = DriveConstants.kStatic;
+        public double maxVel = DriveConstants.MAX_VEL;
+        public double maxAccel = DriveConstants.MAX_ACCEL;
+        public double maxAngVel = DriveConstants.MAX_ANG_VEL;
+        public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL;
+
+        public double mecTransP = SampleMecanumDrive.TRANSLATIONAL_PID.kP;
+        public double mecTransI = SampleMecanumDrive.TRANSLATIONAL_PID.kI;
+        public double mecTransD = SampleMecanumDrive.TRANSLATIONAL_PID.kD;
+        public double mecHeadingP = SampleMecanumDrive.HEADING_PID.kP;
+        public double mecHeadingI = SampleMecanumDrive.HEADING_PID.kI;
+        public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD;
+        public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER;
+
+        public double tankAxialP = SampleTankDrive.AXIAL_PID.kP;
+        public double tankAxialI = SampleTankDrive.AXIAL_PID.kI;
+        public double tankAxialD = SampleTankDrive.AXIAL_PID.kD;
+        public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP;
+        public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI;
+        public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD;
+        public double tankHeadingP = SampleTankDrive.HEADING_PID.kP;
+        public double tankHeadingI = SampleTankDrive.HEADING_PID.kI;
+        public double tankHeadingD = SampleTankDrive.HEADING_PID.kD;
+
+        public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV;
+        public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS;
+        public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;
+        public double trackingLateralDistance = StandardTrackingWheelLocalizer.LATERAL_DISTANCE;
+        public double trackingForwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET;
+
+        public List<Long> nsTimes = new ArrayList<>();
+
+        public List<Double> targetXs = new ArrayList<>();
+        public List<Double> targetYs = new ArrayList<>();
+        public List<Double> targetHeadings = new ArrayList<>();
+
+        public List<Double> xs = new ArrayList<>();
+        public List<Double> ys = new ArrayList<>();
+        public List<Double> headings = new ArrayList<>();
+
+        public LogFile(String opModeName) {
+            this.opModeName = opModeName;
+        }
+    }
+
+    public static void recordTargetPose(Pose2d targetPose) {
+        log.targetXs.add(targetPose.getX());
+        log.targetYs.add(targetPose.getY());
+        log.targetHeadings.add(targetPose.getHeading());
+    }
+
+    public static void recordPose(Pose2d pose) {
+        // arbitrarily add time here
+        log.nsTimes.add(System.nanoTime());
+
+        log.xs.add(pose.getX());
+        log.ys.add(pose.getY());
+        log.headings.add(pose.getHeading());
+    }
+
+    private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {
+        @SuppressLint("SimpleDateFormat")
+        final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS");
+
+        final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory())
+                .writerWithDefaultPrettyPrinter();
+
+        @Override
+        public void onOpModePreInit(OpMode opMode) {
+            log = new LogFile(opMode.getClass().getCanonicalName());
+
+            // clean up old files
+            File[] fs = Objects.requireNonNull(ROOT.listFiles());
+            Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified()));
+            long totalSizeBytes = 0;
+            for (File f : fs) {
+                totalSizeBytes += f.length();
+            }
+
+            int i = 0;
+            while (i < fs.length && totalSizeBytes >= 8 * 1000 * 1000) {
+                totalSizeBytes -= fs[i].length();
+                if (!fs[i].delete()) {
+                    RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath());
+                }
+                ++i;
+            }
+        }
+
+        @Override
+        public void onOpModePreStart(OpMode opMode) {
+            log.nsStart = System.nanoTime();
+        }
+
+        @Override
+        public void onOpModePostStop(OpMode opMode) {
+            log.nsStop = System.nanoTime();
+
+            if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) {
+                String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json";
+                File file = new File(ROOT, filename);
+                try {
+                    jsonWriter.writeValue(file, log);
+                } catch (IOException e) {
+                    RobotLog.setGlobalErrorMsg(new RuntimeException(e),
+                            "Unable to write data to " + file.getAbsolutePath());
+                }
+            }
+        }
+    };
+
+    @WebHandlerRegistrar
+    public static void registerRoutes(Context context, WebHandlerManager manager) {
+        //noinspection ResultOfMethodCallIgnored
+        ROOT.mkdirs();
+
+        // op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves
+        // don't use @OnCreateEventLoop because it's unreliable
+        OpModeManagerImpl.getOpModeManagerOfActivity(
+                AppUtil.getInstance().getActivity()
+        ).registerListener(notifHandler);
+
+        manager.register("/logs", session -> {
+            final StringBuilder sb = new StringBuilder();
+            sb.append("<!doctype html><html><head><title>Logs</title></head><body><ul>");
+            File[] fs = Objects.requireNonNull(ROOT.listFiles());
+            Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified()));
+            for (File f : fs) {
+                sb.append("<li><a href=\"/logs/download?file=");
+                sb.append(f.getName());
+                sb.append("\" download=\"");
+                sb.append(f.getName());
+                sb.append("\">");
+                sb.append(f.getName());
+                sb.append("</a></li>");
+            }
+            sb.append("</ul></body></html>");
+            return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK,
+                    NanoHTTPD.MIME_HTML, sb.toString());
+        });
+
+        manager.register("/logs/download", session -> {
+            final String[] pairs = session.getQueryParameterString().split("&");
+            if (pairs.length != 1) {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+                        NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length);
+            }
+
+            final String[] parts = pairs[0].split("=");
+            if (!parts[0].equals("file")) {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+                        NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]);
+            }
+
+            File f = new File(ROOT, parts[1]);
+            if (!f.exists()) {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
+                        NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist");
+            }
+
+            return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
+                    "application/json", new FileInputStream(f));
+        });
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
new file mode 100644
index 0000000..2c558f1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * Utility functions for log files.
+ */
+public class LoggingUtil {
+    public static final File ROAD_RUNNER_FOLDER =
+            new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
+
+    private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
+
+    private static void buildLogList(List<File> logFiles, File dir) {
+        for (File file : dir.listFiles()) {
+            if (file.isDirectory()) {
+                buildLogList(logFiles, file);
+            } else {
+                logFiles.add(file);
+            }
+        }
+    }
+
+    private static void pruneLogsIfNecessary() {
+        List<File> logFiles = new ArrayList<>();
+        buildLogList(logFiles, ROAD_RUNNER_FOLDER);
+        Collections.sort(logFiles, (lhs, rhs) ->
+                Long.compare(lhs.lastModified(), rhs.lastModified()));
+
+        long dirSize = 0;
+        for (File file: logFiles) {
+            dirSize += file.length();
+        }
+
+        while (dirSize > LOG_QUOTA) {
+            if (logFiles.size() == 0) break;
+            File fileToRemove = logFiles.remove(0);
+            dirSize -= fileToRemove.length();
+            //noinspection ResultOfMethodCallIgnored
+            fileToRemove.delete();
+        }
+    }
+
+    /**
+     * Obtain a log file with the provided name
+     */
+    public static File getLogFile(String name) {
+        //noinspection ResultOfMethodCallIgnored
+        ROAD_RUNNER_FOLDER.mkdirs();
+
+        pruneLogsIfNecessary();
+
+        return new File(ROAD_RUNNER_FOLDER, name);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java
new file mode 100644
index 0000000..243dd7e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java
@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Collection of utilites for interacting with Lynx modules.
+ */
+public class LynxModuleUtil {
+
+    private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
+
+    /**
+     * Parsed representation of a Lynx module firmware version.
+     */
+    public static class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
+        public final int major;
+        public final int minor;
+        public final int eng;
+
+        public LynxFirmwareVersion(int major, int minor, int eng) {
+            this.major = major;
+            this.minor = minor;
+            this.eng = eng;
+        }
+
+        @Override
+        public boolean equals(Object other) {
+            if (other instanceof LynxFirmwareVersion) {
+                LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
+                return major == otherVersion.major && minor == otherVersion.minor &&
+                        eng == otherVersion.eng;
+            } else {
+                return false;
+            }
+        }
+
+        @Override
+        public int compareTo(LynxFirmwareVersion other) {
+            int majorComp = Integer.compare(major, other.major);
+            if (majorComp == 0) {
+                int minorComp = Integer.compare(minor, other.minor);
+                if (minorComp == 0) {
+                    return Integer.compare(eng, other.eng);
+                } else {
+                    return minorComp;
+                }
+            } else {
+                return majorComp;
+            }
+        }
+
+        @Override
+        public String toString() {
+            return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
+        }
+    }
+
+    /**
+     * Retrieve and parse Lynx module firmware version.
+     * @param module Lynx module
+     * @return parsed firmware version
+     */
+    public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
+        String versionString = module.getNullableFirmwareVersionString();
+        if (versionString == null) {
+            return null;
+        }
+
+        String[] parts = versionString.split("[ :,]+");
+        try {
+            // note: for now, we ignore the hardware entry
+            return new LynxFirmwareVersion(
+                    Integer.parseInt(parts[3]),
+                    Integer.parseInt(parts[5]),
+                    Integer.parseInt(parts[7])
+            );
+        } catch (NumberFormatException e) {
+            return null;
+        }
+    }
+
+    /**
+     * Exception indicating an outdated Lynx firmware version.
+     */
+    public static class LynxFirmwareVersionException extends RuntimeException {
+        public LynxFirmwareVersionException(String detailMessage) {
+            super(detailMessage);
+        }
+    }
+
+    /**
+     * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
+     * @param hardwareMap hardware map containing Lynx modules
+     */
+    public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
+        Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            LynxFirmwareVersion version = getFirmwareVersion(module);
+            if (version == null || version.compareTo(MIN_VERSION) < 0) {
+                for (String name : hardwareMap.getNamesOf(module)) {
+                    outdatedModules.put(name, version);
+                }
+            }
+        }
+        if (outdatedModules.size() > 0) {
+            StringBuilder msgBuilder = new StringBuilder();
+            msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
+            msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
+                    MIN_VERSION.toString()));
+            for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
+                msgBuilder.append(Misc.formatInvariant(
+                        "\t%s: %s\n", entry.getKey(),
+                        entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
+            }
+            throw new LynxFirmwareVersionException(msgBuilder.toString());
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java
new file mode 100644
index 0000000..c3bf488
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java
@@ -0,0 +1,156 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+
+import org.apache.commons.math3.stat.regression.SimpleRegression;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Various regression utilities.
+ */
+public class RegressionUtil {
+
+    /**
+     * Feedforward parameter estimates from the ramp regression and additional summary statistics
+     */
+    public static class RampResult {
+        public final double kV, kStatic, rSquare;
+
+        public RampResult(double kV, double kStatic, double rSquare) {
+            this.kV = kV;
+            this.kStatic = kStatic;
+            this.rSquare = rSquare;
+        }
+    }
+
+    /**
+     * Feedforward parameter estimates from the ramp regression and additional summary statistics
+     */
+    public static class AccelResult {
+        public final double kA, rSquare;
+
+        public AccelResult(double kA, double rSquare) {
+            this.kA = kA;
+            this.rSquare = rSquare;
+        }
+    }
+
+    /**
+     * Numerically compute dy/dx from the given x and y values. The returned list is padded to match
+     * the length of the original sequences.
+     *
+     * @param x x-values
+     * @param y y-values
+     * @return derivative values
+     */
+    private static List<Double> numericalDerivative(List<Double> x, List<Double> y) {
+        List<Double> deriv = new ArrayList<>(x.size());
+        for (int i = 1; i < x.size() - 1; i++) {
+            deriv.add(
+                    (y.get(i + 1) - y.get(i - 1)) /
+                    (x.get(i + 1) - x.get(i - 1))
+            );
+        }
+        // copy endpoints to pad output
+        deriv.add(0, deriv.get(0));
+        deriv.add(deriv.get(deriv.size() - 1));
+        return deriv;
+    }
+
+    /**
+     * Run regression to compute velocity and static feedforward from ramp test data.
+     *
+     * Here's the general procedure for gathering the requisite data:
+     *   1. Slowly ramp the motor power/voltage and record encoder values along the way.
+     *   2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
+     *      (kV) and an optional intercept (kStatic).
+     *
+     * @param timeSamples time samples
+     * @param positionSamples position samples
+     * @param powerSamples power samples
+     * @param fitStatic fit kStatic
+     * @param file log file
+     */
+    public static RampResult fitRampData(List<Double> timeSamples, List<Double> positionSamples,
+                                         List<Double> powerSamples, boolean fitStatic,
+                                         @Nullable File file) {
+        if (file != null) {
+            try (PrintWriter pw = new PrintWriter(file)) {
+                pw.println("time,position,power");
+                for (int i = 0; i < timeSamples.size(); i++) {
+                    double time = timeSamples.get(i);
+                    double pos = positionSamples.get(i);
+                    double power = powerSamples.get(i);
+                    pw.println(time + "," + pos + "," + power);
+                }
+            } catch (FileNotFoundException e) {
+                // ignore
+            }
+        }
+
+        List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
+
+        SimpleRegression rampReg = new SimpleRegression(fitStatic);
+        for (int i = 0; i < timeSamples.size(); i++) {
+            double vel = velSamples.get(i);
+            double power = powerSamples.get(i);
+
+            rampReg.addData(vel, power);
+        }
+
+        return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
+                              rampReg.getRSquare());
+    }
+
+    /**
+     * Run regression to compute acceleration feedforward.
+     *
+     * @param timeSamples time samples
+     * @param positionSamples position samples
+     * @param powerSamples power samples
+     * @param rampResult ramp result
+     * @param file log file
+     */
+    public static AccelResult fitAccelData(List<Double> timeSamples, List<Double> positionSamples,
+                                           List<Double> powerSamples, RampResult rampResult,
+                                           @Nullable File file) {
+        if (file != null) {
+            try (PrintWriter pw = new PrintWriter(file)) {
+                pw.println("time,position,power");
+                for (int i = 0; i < timeSamples.size(); i++) {
+                    double time = timeSamples.get(i);
+                    double pos = positionSamples.get(i);
+                    double power = powerSamples.get(i);
+                    pw.println(time + "," + pos + "," + power);
+                }
+            } catch (FileNotFoundException e) {
+                // ignore
+            }
+        }
+
+        List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
+        List<Double> accelSamples = numericalDerivative(timeSamples, velSamples);
+
+        SimpleRegression accelReg = new SimpleRegression(false);
+        for (int i = 0; i < timeSamples.size(); i++) {
+            double vel = velSamples.get(i);
+            double accel = accelSamples.get(i);
+            double power = powerSamples.get(i);
+
+            double powerFromVel = Kinematics.calculateMotorFeedforward(
+                    vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic);
+            double powerFromAccel = power - powerFromVel;
+
+            accelReg.addData(accel, powerFromAccel);
+        }
+
+        return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare());
+    }
+}
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
index a108f41..f7e61ab 100644
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -1,6 +1,7 @@
 repositories {
     mavenCentral()
     google() // Needed for androidx
+    maven { url = 'https://maven.brott.dev/' }
     flatDir {
         dirs rootProject.file('libs')
     }
@@ -18,5 +19,6 @@ dependencies {
     implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0'
     implementation 'androidx.appcompat:appcompat:1.2.0'
     implementation 'org.firstinspires.ftc:gameAssets-PowerPlay:1.0.0'
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
 }
 

From ee90213d64cc529e224436e2a99299bc267679ac Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Thu, 9 Feb 2023 22:19:24 -0500
Subject: [PATCH 02/43] started to integrate LinearSlide

---
 .../ftc/teamcode/slide/LinearSlide.java       | 142 ++++++++++++++++++
 .../ftc/teamcode/slide/SlideConstants.java    |  18 +++
 2 files changed, 160 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
new file mode 100644
index 0000000..c95e1a7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
@@ -0,0 +1,142 @@
+package org.firstinspires.ftc.teamcode.slide;
+
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.GRIP_MAX;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.GRIP_MIN;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.HOLD_POWER;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.INCREMENT_STEPS;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.LOW_HEIGHT;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MAX_HEIGHT;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MAX_POWER;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MEDIUM_HEIGHT;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MIN_HEIGHT;
+import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MIN_POWER;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+
+public class LinearSlide {
+    // Hardware
+    public DcMotorEx slideMotor;
+    public Servo leftGripper, rightGripper;
+
+    private boolean gripperClosed = false;
+    private boolean dPadPressed = false;
+    private int armMotorSteps = 0;
+    private final LinearOpMode opMode;
+
+    public LinearSlide(LinearOpMode opMode) {
+        this.opMode = opMode;
+
+        // Initialize slide motor
+        slideMotor = opMode.hardwareMap.get(DcMotorEx.class, "motor_slide");
+        slideMotor.setDirection(DcMotorEx.Direction.REVERSE);
+        slideMotor.setTargetPosition(MIN_HEIGHT);
+        slideMotor.setPower(MAX_POWER);
+        slideMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
+        slideMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
+        slideMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
+
+        // Initialize gripper servos
+        leftGripper = opMode.hardwareMap.get(Servo.class, "grip1");
+        rightGripper = opMode.hardwareMap.get(Servo.class, "grip2");
+    }
+
+    public void updateTelemetry() {
+        // Add some debug information
+        opMode.telemetry.addData("Target arm motor steps:", armMotorSteps);
+        opMode.telemetry.addData("Actual arm motor steps:", slideMotor.getCurrentPosition());
+        opMode.telemetry.addData("Arm Power:", slideMotor.getPower());
+        opMode.telemetry.addData("Arm Current (A):", slideMotor.getCurrent(CurrentUnit.AMPS));
+        opMode.telemetry.addData("Arm ZeroPower:", slideMotor.getZeroPowerBehavior());
+    }
+
+    public void readGamepad() {
+        // Set arm steps to predefined height with buttons
+        if (opMode.gamepad2.a) {
+            armMotorSteps = MIN_HEIGHT;
+        } else if (opMode.gamepad2.b) {
+            armMotorSteps = LOW_HEIGHT;
+        } else if (opMode.gamepad2.x) {
+            armMotorSteps = MEDIUM_HEIGHT;
+        } else if (opMode.gamepad2.y) {
+            armMotorSteps = MAX_HEIGHT;
+        }
+
+        // Increase arm steps by DPAD increments
+        if (opMode.gamepad2.dpad_up) {
+            armMotorSteps += INCREMENT_STEPS;
+            dPadPressed = true;
+        } else if (opMode.gamepad2.dpad_down) {
+            armMotorSteps -= INCREMENT_STEPS;
+            dPadPressed = true;
+        } else if (dPadPressed) {
+            armMotorSteps = slideMotor.getCurrentPosition();
+            dPadPressed = false;
+        }
+
+        setSlide(armMotorSteps);
+
+        // If motor is busy, run motor at max power.
+        // If motor is not busy, hold at current position or stop at lowest height
+        if (slideMotor.isBusy()) {
+            slideMotor.setPower(MAX_POWER);
+        } else {
+            slideMotor.setPower(HOLD_POWER);
+            if (slideMotor.getCurrentPosition() == MIN_HEIGHT) {
+                slideMotor.setPower(MIN_POWER);
+            }
+        }
+
+        if (opMode.gamepad1.right_trigger > 0.5) {
+            gripperClosed = true;
+        } else if (opMode.gamepad1.left_trigger > 0.5) {
+            gripperClosed = false;
+        }
+
+        if (gripperClosed) {
+            grab();
+        } else {
+            release();
+        }
+    }
+
+    public void grab() {
+        leftGripper.setPosition(GRIP_MAX + 0.1);
+        rightGripper.setPosition(GRIP_MIN - 0.1);
+    }
+
+    public void grabAndWait() {
+        grab();
+        while (leftGripper.getPosition() < GRIP_MAX || rightGripper.getPosition() > GRIP_MIN) {
+            opMode.idle();
+        }
+        opMode.sleep(300);
+    }
+
+    public void release() {
+        leftGripper.setPosition(GRIP_MIN);
+        rightGripper.setPosition(GRIP_MAX);
+    }
+
+    public void releaseAndWait() {
+        release();
+        while (leftGripper.getPosition() > GRIP_MIN || rightGripper.getPosition() < GRIP_MAX) {
+            opMode.idle();
+        }
+    }
+
+    public void setSlide(int steps) {
+        armMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
+        slideMotor.setTargetPosition(armMotorSteps);
+    }
+
+    public void setSlideAndWait(int steps) {
+        setSlide(steps);
+        //opMode.blockOn(slideMotor);
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
new file mode 100644
index 0000000..f86d768
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.slide;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class SlideConstants {
+    // TODO: Add comments to these constants + add some conversion functions to make it more like DriveConstants
+    public static final double MIN_POWER = 0;
+    public static final double HOLD_POWER = 0.1;
+    public static final double MAX_POWER = 1;
+    public static final int MIN_HEIGHT = 0;
+    public static final int LOW_HEIGHT = 1600;
+    public static final int MEDIUM_HEIGHT = 2760;
+    public static final int MAX_HEIGHT = 3870;
+    public static final int INCREMENT_STEPS = 12;
+    public static final double GRIP_MIN = 0;
+    public static final double GRIP_MAX = 0.6;
+}

From 4cfa343549b74be882cea5cdd9390b73e06e536c Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Thu, 9 Feb 2023 22:39:03 -0500
Subject: [PATCH 03/43] added one player teleop test

---
 .../ftc/teamcode/opmode/TeleOpTest.java       | 48 +++++++++++++++++++
 1 file changed, 48 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
new file mode 100644
index 0000000..765b8f4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+@TeleOp(name = "TeleOp Test")
+public class TeleOpTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        double limiter = 0.75;
+
+        while (opModeIsActive()) {
+            double forward = gamepad1.left_stick_y * limiter;
+            double strafe = gamepad1.left_stick_x * limiter;
+            double rotate = gamepad1.right_stick_x * limiter;
+
+            if (Math.abs(gamepad1.left_stick_y) < 0.01) {
+                forward = 0;
+            }
+            if (Math.abs(gamepad1.left_stick_x) < 0.01) {
+                strafe = 0;
+            }
+            if (Math.abs(gamepad1.right_stick_x) < 0.01) {
+                rotate = 0;
+            }
+
+            drive.setMotorPowers(
+                    forward + strafe + rotate,
+                    forward - strafe + rotate,
+                    forward - strafe - rotate,
+                    forward + strafe - rotate
+            );
+        }
+    }
+}

From 305b812e02b95ba92279875fade046bc275eba55 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Thu, 9 Feb 2023 23:04:49 -0500
Subject: [PATCH 04/43] added field centric to teleop test

---
 .../ftc/teamcode/opmode/TeleOpTest.java           | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
index 765b8f4..18d4bf9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
@@ -26,6 +26,7 @@ public void runOpMode() throws InterruptedException {
             double forward = gamepad1.left_stick_y * limiter;
             double strafe = gamepad1.left_stick_x * limiter;
             double rotate = gamepad1.right_stick_x * limiter;
+            double heading = drive.getExternalHeading();
 
             if (Math.abs(gamepad1.left_stick_y) < 0.01) {
                 forward = 0;
@@ -37,12 +38,18 @@ public void runOpMode() throws InterruptedException {
                 rotate = 0;
             }
 
+            double fieldFoward = strafe * Math.sin(-heading) + forward * Math.cos(-heading);
+            double fieldStrafe = strafe * Math.cos(-heading) - forward * Math.sin(-heading);
+
             drive.setMotorPowers(
-                    forward + strafe + rotate,
-                    forward - strafe + rotate,
-                    forward - strafe - rotate,
-                    forward + strafe - rotate
+                    fieldFoward + fieldStrafe + rotate,
+                    fieldFoward - fieldStrafe + rotate,
+                    fieldFoward - fieldStrafe - rotate,
+                    fieldFoward + fieldStrafe - rotate
             );
+
+
+            drive.update();
         }
     }
 }

From f4a0e11e96dd914e846781557b6b7af9e73df738 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Thu, 9 Feb 2023 23:22:38 -0500
Subject: [PATCH 05/43] added basic one-cone auto

---
 .../ftc/teamcode/opmode/AutoTest.java         | 41 +++++++++++++++++++
 1 file changed, 41 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
new file mode 100644
index 0000000..b928ae2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.slide.LinearSlide;
+
+@Autonomous(name = "Auto Test")
+public class AutoTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        LinearSlide slide = new LinearSlide(this);
+
+        slide.release();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+
+        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+                .addDisplacementMarker(slide::grab)
+                .forward(5.5)
+                .strafeLeft(37.5)
+                .addDisplacementMarker(() -> slide.setSlideAndWait(1900))
+                .forward(6.5)
+                .addDisplacementMarker(slide::release)
+                .build();
+
+        drive.followTrajectory(trajectory);
+    }
+}

From 7858da5bae0f2f5e80540f756e01b35330cc8940 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 10 Feb 2023 09:40:57 -0500
Subject: [PATCH 06/43] fixed hardware map names for drive

---
 .../teamcode/drive/SampleMecanumDrive.java    | 32 +++++++++----------
 .../ftc/teamcode/opmode/AutoTest.java         |  2 +-
 .../ftc/teamcode/opmode/TeleOpTest.java       | 11 +++----
 3 files changed, 22 insertions(+), 23 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
index e59e116..7ee54b7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -1,5 +1,17 @@
 package org.firstinspires.ftc.teamcode.drive;
 
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.config.Config;
@@ -37,18 +49,6 @@
 import java.util.Arrays;
 import java.util.List;
 
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
-
 /*
  * Simple mecanum drive hardware implementation for REV hardware.
  */
@@ -99,10 +99,10 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
                 RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
         imu.initialize(parameters);
 
-        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
-        leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
-        rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
-        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+        leftFront = hardwareMap.get(DcMotorEx.class, "motor_front_left");
+        leftRear = hardwareMap.get(DcMotorEx.class, "motor_back_left");
+        rightRear = hardwareMap.get(DcMotorEx.class, "motor_back_right");
+        rightFront = hardwareMap.get(DcMotorEx.class, "motor_front_right");
 
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
index b928ae2..32a771a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
@@ -26,7 +26,7 @@ public void runOpMode() throws InterruptedException {
 
         if (isStopRequested()) return;
 
-
+        // First Cone
         Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
                 .addDisplacementMarker(slide::grab)
                 .forward(5.5)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
index 18d4bf9..96ed330 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
@@ -38,17 +38,16 @@ public void runOpMode() throws InterruptedException {
                 rotate = 0;
             }
 
-            double fieldFoward = strafe * Math.sin(-heading) + forward * Math.cos(-heading);
+            double fieldForward = strafe * Math.sin(-heading) + forward * Math.cos(-heading);
             double fieldStrafe = strafe * Math.cos(-heading) - forward * Math.sin(-heading);
 
             drive.setMotorPowers(
-                    fieldFoward + fieldStrafe + rotate,
-                    fieldFoward - fieldStrafe + rotate,
-                    fieldFoward - fieldStrafe - rotate,
-                    fieldFoward + fieldStrafe - rotate
+                    fieldForward + fieldStrafe + rotate,
+                    fieldForward - fieldStrafe + rotate,
+                    fieldForward - fieldStrafe - rotate,
+                    fieldForward + fieldStrafe - rotate
             );
 
-
             drive.update();
         }
     }

From b7c3a209dd69f5dc358099d6f72e90d7a75f798b Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 10 Feb 2023 10:33:15 -0500
Subject: [PATCH 07/43] fixed motor directions and IMU orientation

---
 .../firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
index 7ee54b7..438ca28 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -96,7 +96,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
         // details
         IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
                 RevHubOrientationOnRobot.LogoFacingDirection.UP,
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
+                RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
         imu.initialize(parameters);
 
         leftFront = hardwareMap.get(DcMotorEx.class, "motor_front_left");
@@ -104,6 +104,9 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
         rightRear = hardwareMap.get(DcMotorEx.class, "motor_back_right");
         rightFront = hardwareMap.get(DcMotorEx.class, "motor_front_right");
 
+        leftRear.setDirection(DcMotorEx.Direction.REVERSE);
+        leftFront.setDirection(DcMotorEx.Direction.REVERSE);
+
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 
         for (DcMotorEx motor : motors) {

From b9134fc0eb038b1992ac6f65d7090c503e39ec3c Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 10 Feb 2023 10:39:15 -0500
Subject: [PATCH 08/43] removed some TODO's in mechanum drive

---
 .../ftc/teamcode/drive/SampleMecanumDrive.java           | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
index 438ca28..8e97d27 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -90,10 +90,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
             module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
         }
 
-        // TODO: adjust the names of the following hardware devices to match your configuration
         imu = hardwareMap.get(IMU.class, "imu");
-        // TODO: Adjust the orientations here to match your robot. See the FTC SDK documentation for
-        // details
         IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
                 RevHubOrientationOnRobot.LogoFacingDirection.UP,
                 RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
@@ -104,9 +101,6 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
         rightRear = hardwareMap.get(DcMotorEx.class, "motor_back_right");
         rightFront = hardwareMap.get(DcMotorEx.class, "motor_front_right");
 
-        leftRear.setDirection(DcMotorEx.Direction.REVERSE);
-        leftFront.setDirection(DcMotorEx.Direction.REVERSE);
-
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 
         for (DcMotorEx motor : motors) {
@@ -125,7 +119,8 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
             setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
         }
 
-        // TODO: reverse any motors using DcMotor.setDirection()
+        leftRear.setDirection(DcMotorEx.Direction.REVERSE);
+        leftFront.setDirection(DcMotorEx.Direction.REVERSE);
 
         // TODO: if desired, use setLocalizer() to change the localization method
         // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));

From 570a52b50a0310e0b6715f22506da702216bb532 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 10 Feb 2023 16:03:20 -0500
Subject: [PATCH 09/43] hopefully fixed teleop test

---
 .../ftc/teamcode/drive/DriveConstants.java    | 18 +++---
 .../ftc/teamcode/opmode/AutoTest.java         |  6 +-
 .../ftc/teamcode/opmode/TeleOpTest.java       | 61 +++++++++++++++----
 3 files changed, 60 insertions(+), 25 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
index 7d3913d..4fc775c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -10,7 +10,7 @@
  *
  * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
  * fields may also be edited through the dashboard (connect to the robot's WiFi network and
- * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * navigate to http://192.168.49.1:8080/dash). Make sure to save the values here after you
  * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
  *
  * These are not the only parameters; some are located in the localizer classes, drive base classes,
@@ -35,7 +35,7 @@ public class DriveConstants {
      */
     public static final boolean RUN_USING_ENCODER = true;
     public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
-      getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+            getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
 
     /*
      * These are physical constants that can be determined from your robot (including the track
@@ -74,11 +74,11 @@ public class DriveConstants {
      * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
      * This is capped at 85% because there are a number of variables that will prevent your bot from actually
      * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
-     * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically 
+     * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
      * max velocity. The theoretically maximum velocity is 18551.732937978446 in/s.
      * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
      * affected if it is aiming for a velocity not actually possible.
-     * 
+     *
      * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
      * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
      * to degrade. As of now, it simply mirrors the velocity, resulting in 15768.972997281679 in/s/s
@@ -91,13 +91,13 @@ public class DriveConstants {
      * velocity (85% of max velocity) exceeding 90in/s.
      * (Your recommended velocity was 15768.972997281679in/s)
      * This is simply insanely fast for an FTC bot and chances are your bot cannot properly reach these speeds.
-     * 
+     *
      * Just to be safe, LearnRoadRunner.com has arbitrarily limited your velocity to 90in/s.
      * You are free to increase it yourself. If you do run into issues, please lower the maximum velocity.
-     * 
+     *
      * A documented case of a similar error which served as an impetus for this reasoning can be found here:
      * https://github.com/acmerobotics/road-runner-quickstart/issues/91
-     
+
      */
     public static double MAX_VEL = 90;
     public static double MAX_ACCEL = 90;
@@ -114,7 +114,7 @@ public static double rpmToVelocity(double rpm) {
     }
 
     public static double getMotorVelocityF(double ticksPerSecond) {
-      // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
-      return 32767 / ticksPerSecond;
+        // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
+        return 32767 / ticksPerSecond;
     }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
index 32a771a..927ba29 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
@@ -28,12 +28,12 @@ public void runOpMode() throws InterruptedException {
 
         // First Cone
         Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
-                .addDisplacementMarker(slide::grab)
+                //.addDisplacementMarker(slide::grab)
                 .forward(5.5)
                 .strafeLeft(37.5)
-                .addDisplacementMarker(() -> slide.setSlideAndWait(1900))
+                //.addDisplacementMarker(() -> slide.setSlideAndWait(1900))
                 .forward(6.5)
-                .addDisplacementMarker(slide::release)
+                //.addDisplacementMarker(slide::release)
                 .build();
 
         drive.followTrajectory(trajectory);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
index 96ed330..ab2e5d1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
@@ -2,6 +2,7 @@
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -21,32 +22,66 @@ public void runOpMode() throws InterruptedException {
         if (isStopRequested()) return;
 
         double limiter = 0.75;
+        boolean field = true;
 
         while (opModeIsActive()) {
-            double forward = gamepad1.left_stick_y * limiter;
+            double heading = drive.getExternalHeading();
+
+            if (gamepad1.a) {
+                limiter = 0.75;
+            } else if (gamepad1.b) {
+                limiter = 0.25;
+            }
+            if (gamepad1.x) {
+                drive.setPoseEstimate(new Pose2d());
+                field = true;
+            } else if (gamepad1.y) {
+                field = false;
+            }
+
+            double forward = -gamepad1.left_stick_y * limiter;
             double strafe = gamepad1.left_stick_x * limiter;
             double rotate = gamepad1.right_stick_x * limiter;
-            double heading = drive.getExternalHeading();
 
-            if (Math.abs(gamepad1.left_stick_y) < 0.01) {
-                forward = 0;
+            if (Math.abs(-gamepad1.left_stick_y) < 0.01) {
+                forward = -gamepad2.left_stick_y * 0.4;
+                if (Math.abs(gamepad2.left_stick_y) < 0.02) {
+                    forward = 0;
+                }
             }
             if (Math.abs(gamepad1.left_stick_x) < 0.01) {
-                strafe = 0;
+                strafe = gamepad2.left_stick_x * 0.4;
+                if (Math.abs(gamepad2.left_stick_x) < 0.02) {
+                    strafe = 0;
+                }
             }
             if (Math.abs(gamepad1.right_stick_x) < 0.01) {
-                rotate = 0;
+                rotate = gamepad2.right_stick_x * 0.4;
+                if (Math.abs(gamepad2.right_stick_x) < 0.02) {
+                    rotate = 0;
+                }
             }
-
             double fieldForward = strafe * Math.sin(-heading) + forward * Math.cos(-heading);
             double fieldStrafe = strafe * Math.cos(-heading) - forward * Math.sin(-heading);
 
-            drive.setMotorPowers(
-                    fieldForward + fieldStrafe + rotate,
-                    fieldForward - fieldStrafe + rotate,
-                    fieldForward - fieldStrafe - rotate,
-                    fieldForward + fieldStrafe - rotate
-            );
+            if (!field) {
+                drive.setMotorPowers(
+                        forward + strafe + rotate,
+                        forward - strafe + rotate,
+                        forward - strafe - rotate,
+                        forward + strafe - rotate
+                );
+            } else {
+                drive.setMotorPowers(
+                        fieldForward + fieldStrafe + rotate,
+                        fieldForward - fieldStrafe + rotate,
+                        fieldForward - fieldStrafe - rotate,
+                        fieldForward + fieldStrafe - rotate
+                );
+            }
+            telemetry.addData("Limiter: ", limiter);
+            telemetry.addData("Heading: ", heading);
+            telemetry.addData("Field Centric?: ", field);
 
             drive.update();
         }

From 6861d602c78e96dc32eb74a7fc959bb77d016980 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Sat, 11 Feb 2023 11:16:48 -0500
Subject: [PATCH 10/43] fixed gear ratio in drive constants

---
 .../ftc/teamcode/drive/DriveConstants.java    | 30 ++++++-------------
 .../teamcode/drive/SampleMecanumDrive.java    | 18 +++++++++++
 .../ftc/teamcode/slide/LinearSlide.java       | 30 ++++++++++---------
 .../ftc/teamcode/slide/SlideConstants.java    | 20 ++++++++++++-
 4 files changed, 62 insertions(+), 36 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
index 4fc775c..ed93d1e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -10,7 +10,7 @@
  *
  * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
  * fields may also be edited through the dashboard (connect to the robot's WiFi network and
- * navigate to http://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
  * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
  *
  * These are not the only parameters; some are located in the localizer classes, drive base classes,
@@ -46,7 +46,7 @@ public class DriveConstants {
      * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
      */
     public static double WHEEL_RADIUS = 1.4763; // in
-    public static double GEAR_RATIO = 20; // output (wheel) speed / input (motor) speed
+    public static double GEAR_RATIO = 0.05; // output (wheel) speed / input (motor) speed
     public static double TRACK_WIDTH = 13.75; // in
 
     /*
@@ -70,39 +70,27 @@ public class DriveConstants {
      * Note from LearnRoadRunner.com:
      * The velocity and acceleration constraints were calculated based on the following equation:
      * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
-     * Resulting in 15768.972997281679 in/s.
+     * Resulting in 39.4224324932042 in/s.
      * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
      * This is capped at 85% because there are a number of variables that will prevent your bot from actually
      * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
      * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
-     * max velocity. The theoretically maximum velocity is 18551.732937978446 in/s.
+     * max velocity. The theoretically maximum velocity is 46.379332344946114 in/s.
      * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
      * affected if it is aiming for a velocity not actually possible.
      *
      * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
      * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
-     * to degrade. As of now, it simply mirrors the velocity, resulting in 15768.972997281679 in/s/s
+     * to degrade. As of now, it simply mirrors the velocity, resulting in 39.4224324932042 in/s/s
      *
      * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
      * You are free to raise this on your own if you would like. It is best determined through experimentation.
-     *
-     * WARNING: LearnRoadRunner.com's constant generator has capped the calculated recommended velocity at 90in/s.
-     * This message is showing because your gear ratio/motor RPM/etc. configuration, results in a recommended
-     * velocity (85% of max velocity) exceeding 90in/s.
-     * (Your recommended velocity was 15768.972997281679in/s)
-     * This is simply insanely fast for an FTC bot and chances are your bot cannot properly reach these speeds.
-     *
-     * Just to be safe, LearnRoadRunner.com has arbitrarily limited your velocity to 90in/s.
-     * You are free to increase it yourself. If you do run into issues, please lower the maximum velocity.
-     *
-     * A documented case of a similar error which served as an impetus for this reasoning can be found here:
-     * https://github.com/acmerobotics/road-runner-quickstart/issues/91
 
      */
-    public static double MAX_VEL = 90;
-    public static double MAX_ACCEL = 90;
-    public static double MAX_ANG_VEL = Math.toRadians(360);
-    public static double MAX_ANG_ACCEL = Math.toRadians(360);
+    public static double MAX_VEL = 39.4224324932042;
+    public static double MAX_ACCEL = 39.4224324932042;
+    public static double MAX_ANG_VEL = Math.toRadians(164.27192727272728);
+    public static double MAX_ANG_ACCEL = Math.toRadians(164.27192727272728);
 
 
     public static double encoderTicksToInches(double ticks) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
index 8e97d27..fc15dcd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -279,6 +279,24 @@ public double getRawExternalHeading() {
 
     @Override
     public Double getExternalHeadingVelocity() {
+        // TODO: This must be changed to match your configuration
+        //                           | Z axis
+        //                           |
+        //     (Motor Port Side)     |   / X axis
+        //                       ____|__/____
+        //          Y axis     / *   | /    /|   (IO Side)
+        //          _________ /______|/    //      I2C
+        //                   /___________ //     Digital
+        //                  |____________|/      Analog
+        //
+        //                 (Servo Port Side)
+        //
+        // The positive x axis points toward the USB port(s)
+        //
+        // Adjust the axis rotation rate as necessary
+        // Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
+        // flat on a surface
+        
         return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
     }
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
index c95e1a7..932654e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
@@ -19,13 +19,12 @@
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 
 public class LinearSlide {
-    // Hardware
     public DcMotorEx slideMotor;
     public Servo leftGripper, rightGripper;
 
     private boolean gripperClosed = false;
     private boolean dPadPressed = false;
-    private int armMotorSteps = 0;
+    private int targetArmMotorSteps = 0;
     private final LinearOpMode opMode;
 
     public LinearSlide(LinearOpMode opMode) {
@@ -47,7 +46,7 @@ public LinearSlide(LinearOpMode opMode) {
 
     public void updateTelemetry() {
         // Add some debug information
-        opMode.telemetry.addData("Target arm motor steps:", armMotorSteps);
+        opMode.telemetry.addData("Target arm motor steps:", targetArmMotorSteps);
         opMode.telemetry.addData("Actual arm motor steps:", slideMotor.getCurrentPosition());
         opMode.telemetry.addData("Arm Power:", slideMotor.getPower());
         opMode.telemetry.addData("Arm Current (A):", slideMotor.getCurrent(CurrentUnit.AMPS));
@@ -57,28 +56,28 @@ public void updateTelemetry() {
     public void readGamepad() {
         // Set arm steps to predefined height with buttons
         if (opMode.gamepad2.a) {
-            armMotorSteps = MIN_HEIGHT;
+            targetArmMotorSteps = MIN_HEIGHT;
         } else if (opMode.gamepad2.b) {
-            armMotorSteps = LOW_HEIGHT;
+            targetArmMotorSteps = LOW_HEIGHT;
         } else if (opMode.gamepad2.x) {
-            armMotorSteps = MEDIUM_HEIGHT;
+            targetArmMotorSteps = MEDIUM_HEIGHT;
         } else if (opMode.gamepad2.y) {
-            armMotorSteps = MAX_HEIGHT;
+            targetArmMotorSteps = MAX_HEIGHT;
         }
 
         // Increase arm steps by DPAD increments
         if (opMode.gamepad2.dpad_up) {
-            armMotorSteps += INCREMENT_STEPS;
+            targetArmMotorSteps += INCREMENT_STEPS;
             dPadPressed = true;
         } else if (opMode.gamepad2.dpad_down) {
-            armMotorSteps -= INCREMENT_STEPS;
+            targetArmMotorSteps -= INCREMENT_STEPS;
             dPadPressed = true;
         } else if (dPadPressed) {
-            armMotorSteps = slideMotor.getCurrentPosition();
+            targetArmMotorSteps = slideMotor.getCurrentPosition();
             dPadPressed = false;
         }
 
-        setSlide(armMotorSteps);
+        setSlide(targetArmMotorSteps);
 
         // If motor is busy, run motor at max power.
         // If motor is not busy, hold at current position or stop at lowest height
@@ -104,6 +103,10 @@ public void readGamepad() {
         }
     }
 
+    public int getTargetMotorSteps() {
+        return targetArmMotorSteps;
+    }
+
     public void grab() {
         leftGripper.setPosition(GRIP_MAX + 0.1);
         rightGripper.setPosition(GRIP_MIN - 0.1);
@@ -130,13 +133,12 @@ public void releaseAndWait() {
     }
 
     public void setSlide(int steps) {
-        armMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
-        slideMotor.setTargetPosition(armMotorSteps);
+        targetArmMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
+        slideMotor.setTargetPosition(targetArmMotorSteps);
     }
 
     public void setSlideAndWait(int steps) {
         setSlide(steps);
         //opMode.blockOn(slideMotor);
     }
-
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
index f86d768..dfdbdae 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
@@ -4,15 +4,33 @@
 
 @Config
 public class SlideConstants {
-    // TODO: Add comments to these constants + add some conversion functions to make it more like DriveConstants
+    // Minimum power of the arm motor
     public static final double MIN_POWER = 0;
+
+    // Holding power of the arm motor (when it is holding a position)
     public static final double HOLD_POWER = 0.1;
+
+    // Maximum power of the arm motor
     public static final double MAX_POWER = 1;
+
+    // Height (in encoder ticks) that the arm motor should go to reach the floor
     public static final int MIN_HEIGHT = 0;
+
+    // Height (in encoder ticks) that the arm motor should go to reach the low junction
     public static final int LOW_HEIGHT = 1600;
+
+    // Height (in encoder ticks) that the arm motor should go to reach the medium junction
     public static final int MEDIUM_HEIGHT = 2760;
+
+    // Height (in encoder ticks) that the arm motor should go to reach the high junction
     public static final int MAX_HEIGHT = 3870;
+
+    // Number of encoder ticks the arm should go up when the player presses the DPAD
     public static final int INCREMENT_STEPS = 12;
+
+    // Minimum servo grip position
     public static final double GRIP_MIN = 0;
+
+    // Maximum servo grip position
     public static final double GRIP_MAX = 0.6;
 }

From 286520a72e16ad027e4728b8b19376c5b3ada7a4 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Sat, 11 Feb 2023 14:55:41 -0500
Subject: [PATCH 11/43] renamed SampleMechanumDrive

---
 .../ftc/teamcode/drive/DriveConstants.java    |  2 +-
 ...mpleMecanumDrive.java => StrafeDrive.java} |  6 +++---
 .../ftc/teamcode/opmode/AutoTest.java         |  4 ++--
 .../opmode/AutomaticFeedforwardTuner.java     |  4 ++--
 .../ftc/teamcode/opmode/BackAndForth.java     |  4 ++--
 .../opmode/DriveVelocityPIDTuner.java         |  4 ++--
 .../ftc/teamcode/opmode/FollowerPIDTuner.java |  4 ++--
 .../ftc/teamcode/opmode/LocalizationTest.java |  4 ++--
 .../opmode/ManualFeedforwardTuner.java        |  6 +++---
 .../teamcode/opmode/MaxAngularVeloTuner.java  |  4 ++--
 .../ftc/teamcode/opmode/MaxVelocityTuner.java |  4 ++--
 .../opmode/MotorDirectionDebugger.java        |  4 ++--
 .../ftc/teamcode/opmode/SplineTest.java       |  4 ++--
 .../ftc/teamcode/opmode/StrafeTest.java       |  4 ++--
 .../ftc/teamcode/opmode/StraightTest.java     |  4 ++--
 .../ftc/teamcode/opmode/TeleOpTest.java       | 21 ++++++++++---------
 .../ftc/teamcode/opmode/TrackWidthTuner.java  |  4 ++--
 .../TrackingWheelForwardOffsetTuner.java      |  4 ++--
 .../TrackingWheelLateralDistanceTuner.java    |  4 ++--
 .../ftc/teamcode/opmode/TurnTest.java         |  4 ++--
 .../ftc/teamcode/util/LogFiles.java           | 16 +++++++-------
 21 files changed, 58 insertions(+), 57 deletions(-)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/{SampleMecanumDrive.java => StrafeDrive.java} (99%)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
index ed93d1e..706e4a5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -10,7 +10,7 @@
  *
  * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
  * fields may also be edited through the dashboard (connect to the robot's WiFi network and
- * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * navigate to http://192.168.49.1:8080/dash). Make sure to save the values here after you
  * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
  *
  * These are not the only parameters; some are located in the localizer classes, drive base classes,
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StrafeDrive.java
similarity index 99%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StrafeDrive.java
index fc15dcd..280bdcd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StrafeDrive.java
@@ -53,7 +53,7 @@
  * Simple mecanum drive hardware implementation for REV hardware.
  */
 @Config
-public class SampleMecanumDrive extends MecanumDrive {
+public class StrafeDrive extends MecanumDrive {
     public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
     public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
 
@@ -76,7 +76,7 @@ public class SampleMecanumDrive extends MecanumDrive {
     private IMU imu;
     private VoltageSensor batteryVoltageSensor;
 
-    public SampleMecanumDrive(HardwareMap hardwareMap) {
+    public StrafeDrive(HardwareMap hardwareMap) {
         super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
 
         follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
@@ -296,7 +296,7 @@ public Double getExternalHeadingVelocity() {
         // Adjust the axis rotation rate as necessary
         // Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
         // flat on a surface
-        
+
         return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
     }
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
index 927ba29..492f466 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
@@ -8,7 +8,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 import org.firstinspires.ftc.teamcode.slide.LinearSlide;
 
 @Autonomous(name = "Auto Test")
@@ -17,7 +17,7 @@ public class AutoTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
         LinearSlide slide = new LinearSlide(this);
 
         slide.release();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
index d714b78..5db99bd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
@@ -15,7 +15,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 import org.firstinspires.ftc.teamcode.util.LoggingUtil;
 import org.firstinspires.ftc.teamcode.util.RegressionUtil;
 
@@ -47,7 +47,7 @@ public void runOpMode() throws InterruptedException {
 
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         NanoClock clock = NanoClock.system();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
index 807fe6b..9be133e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
@@ -6,7 +6,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /*
  * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
@@ -32,7 +32,7 @@ public class BackAndForth extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
                 .forward(DISTANCE)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
index 0fd7f3a..1081e2e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
@@ -20,7 +20,7 @@
 import com.qualcomm.robotcore.util.RobotLog;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 import java.util.List;
 
@@ -73,7 +73,7 @@ public void runOpMode() {
 
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         Mode mode = Mode.TUNING_MODE;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
index cfb7ad6..e6d6b23 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
@@ -5,7 +5,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
 
 /*
@@ -28,7 +28,7 @@ public class FollowerPIDTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
index 73ac2eb..1a28584 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
@@ -5,7 +5,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /**
  * This is a simple teleop routine for testing localization. Drive the robot around like a normal
@@ -18,7 +18,7 @@
 public class LocalizationTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
index 8aa8b06..9b72624 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
@@ -21,7 +21,7 @@
 import com.qualcomm.robotcore.util.RobotLog;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 import java.util.Objects;
 
@@ -47,7 +47,7 @@ public class ManualFeedforwardTuner extends LinearOpMode {
 
     private FtcDashboard dashboard = FtcDashboard.getInstance();
 
-    private SampleMecanumDrive drive;
+    private StrafeDrive drive;
 
     enum Mode {
         DRIVER_MODE,
@@ -71,7 +71,7 @@ public void runOpMode() {
 
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
 
-        drive = new SampleMecanumDrive(hardwareMap);
+        drive = new StrafeDrive(hardwareMap);
 
         mode = Mode.TUNING_MODE;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
index 79c9df2..b6c83dd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
@@ -10,7 +10,7 @@
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 import java.util.Objects;
 
@@ -32,7 +32,7 @@ public class MaxAngularVeloTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
index ccc4118..54cdbb9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
@@ -12,7 +12,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 import java.util.Objects;
 
@@ -36,7 +36,7 @@ public class MaxVelocityTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
index 0a3b688..da02bb7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
@@ -8,7 +8,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /**
  * This is a simple teleop routine for debugging your motor configuration.
@@ -48,7 +48,7 @@ public class MotorDirectionDebugger extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         telemetry.addLine("Press play to begin the debugging opmode");
         telemetry.update();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
index 506c3b3..8c4de46 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
@@ -6,7 +6,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /*
  * This is an example of a more complex path to really test the tuning.
@@ -15,7 +15,7 @@
 public class SplineTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         waitForStart();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
index 90ff4d7..07689b0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /*
  * This is a simple routine to test translational drive capabilities.
@@ -23,7 +23,7 @@ public class StrafeTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
                 .strafeRight(DISTANCE)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
index bee34af..d0c0648 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /*
  * This is a simple routine to test translational drive capabilities.
@@ -23,7 +23,7 @@ public class StraightTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
                 .forward(DISTANCE)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
index ab2e5d1..e887970 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
@@ -7,7 +7,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 @TeleOp(name = "TeleOp Test")
 public class TeleOpTest extends LinearOpMode {
@@ -15,7 +15,7 @@ public class TeleOpTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         waitForStart();
 
@@ -64,21 +64,22 @@ public void runOpMode() throws InterruptedException {
             double fieldForward = strafe * Math.sin(-heading) + forward * Math.cos(-heading);
             double fieldStrafe = strafe * Math.cos(-heading) - forward * Math.sin(-heading);
 
-            if (!field) {
-                drive.setMotorPowers(
-                        forward + strafe + rotate,
-                        forward - strafe + rotate,
-                        forward - strafe - rotate,
-                        forward + strafe - rotate
-                );
-            } else {
+            if (field) {
                 drive.setMotorPowers(
                         fieldForward + fieldStrafe + rotate,
                         fieldForward - fieldStrafe + rotate,
                         fieldForward - fieldStrafe - rotate,
                         fieldForward + fieldStrafe - rotate
                 );
+            } else {
+                drive.setMotorPowers(
+                        forward + strafe + rotate,
+                        forward - strafe + rotate,
+                        forward - strafe - rotate,
+                        forward + strafe - rotate
+                );
             }
+
             telemetry.addData("Limiter: ", limiter);
             telemetry.addData("Heading: ", heading);
             telemetry.addData("Field Centric?: ", field);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
index b852d3b..4739c65 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
@@ -12,7 +12,7 @@
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
 import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /*
  * This routine determines the effective track width. The procedure works by executing a point turn
@@ -34,7 +34,7 @@ public class TrackWidthTuner extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
         // TODO: if you haven't already, set the localizer to something that doesn't depend on
         // drive encoders for computing the heading
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
index 8be29de..a129da7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
@@ -12,8 +12,8 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /**
  * This routine determines the effective forward offset for the lateral tracking wheel.
@@ -45,7 +45,7 @@ public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
             RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
index 22c8224..5e1ef54 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
@@ -7,8 +7,8 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.util.RobotLog;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /**
  * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
@@ -68,7 +68,7 @@ public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
             RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
index 562b415..682df14 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
@@ -4,7 +4,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /*
  * This is a simple routine to test turning capabilities.
@@ -16,7 +16,7 @@ public class TurnTest extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        StrafeDrive drive = new StrafeDrive(hardwareMap);
 
         waitForStart();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
index 66e5e9d..7b2ddab 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
@@ -16,9 +16,9 @@
 import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
 import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
 import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.drive.SampleTankDrive;
 import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 import java.io.File;
 import java.io.FileInputStream;
@@ -65,13 +65,13 @@ public static class LogFile {
         public double maxAngVel = DriveConstants.MAX_ANG_VEL;
         public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL;
 
-        public double mecTransP = SampleMecanumDrive.TRANSLATIONAL_PID.kP;
-        public double mecTransI = SampleMecanumDrive.TRANSLATIONAL_PID.kI;
-        public double mecTransD = SampleMecanumDrive.TRANSLATIONAL_PID.kD;
-        public double mecHeadingP = SampleMecanumDrive.HEADING_PID.kP;
-        public double mecHeadingI = SampleMecanumDrive.HEADING_PID.kI;
-        public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD;
-        public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER;
+        public double mecTransP = StrafeDrive.TRANSLATIONAL_PID.kP;
+        public double mecTransI = StrafeDrive.TRANSLATIONAL_PID.kI;
+        public double mecTransD = StrafeDrive.TRANSLATIONAL_PID.kD;
+        public double mecHeadingP = StrafeDrive.HEADING_PID.kP;
+        public double mecHeadingI = StrafeDrive.HEADING_PID.kI;
+        public double mecHeadingD = StrafeDrive.HEADING_PID.kD;
+        public double mecLateralMultiplier = StrafeDrive.LATERAL_MULTIPLIER;
 
         public double tankAxialP = SampleTankDrive.AXIAL_PID.kP;
         public double tankAxialI = SampleTankDrive.AXIAL_PID.kI;

From a0ae67892257af9c23218b112997383de53418c8 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Sun, 19 Feb 2023 15:46:59 -0500
Subject: [PATCH 12/43] fixed gear ratio and tuned PIDF

---
 .../ftc/teamcode/drive/DriveConstants.java    |  19 +-
 .../ftc/teamcode/opmode/AutoTest.java         |   4 +-
 .../opmode/DriveVelocityPIDTuner.java         |   4 +-
 .../ftc/teamcode/opmode/FollowerPIDTuner.java |   4 +-
 .../opmode/ManualFeedforwardTuner.java        |   2 +-
 .../ftc/teamcode/slide/LinearSlide.java       | 165 ++++++++----------
 6 files changed, 86 insertions(+), 112 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
index 706e4a5..6c9d919 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -34,8 +34,7 @@ public class DriveConstants {
      * from DriveVelocityPIDTuner.
      */
     public static final boolean RUN_USING_ENCODER = true;
-    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
-            getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(30, 0, 12, 12.759179677930504); // f = getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
 
     /*
      * These are physical constants that can be determined from your robot (including the track
@@ -46,7 +45,7 @@ public class DriveConstants {
      * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
      */
     public static double WHEEL_RADIUS = 1.4763; // in
-    public static double GEAR_RATIO = 0.05; // output (wheel) speed / input (motor) speed
+    public static double GEAR_RATIO = 0.05291; // output (wheel) speed / input (motor) speed
     public static double TRACK_WIDTH = 13.75; // in
 
     /*
@@ -70,27 +69,27 @@ public class DriveConstants {
      * Note from LearnRoadRunner.com:
      * The velocity and acceleration constraints were calculated based on the following equation:
      * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
-     * Resulting in 39.4224324932042 in/s.
+     * Resulting in 41.716818064308676 in/s.
      * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
      * This is capped at 85% because there are a number of variables that will prevent your bot from actually
      * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
      * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
-     * max velocity. The theoretically maximum velocity is 46.379332344946114 in/s.
+     * max velocity. The theoretically maximum velocity is 49.07860948742197 in/s.
      * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
      * affected if it is aiming for a velocity not actually possible.
      *
      * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
      * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
-     * to degrade. As of now, it simply mirrors the velocity, resulting in 39.4224324932042 in/s/s
+     * to degrade. As of now, it simply mirrors the velocity, resulting in 41.716818064308676 in/s/s
      *
      * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
      * You are free to raise this on your own if you would like. It is best determined through experimentation.
 
      */
-    public static double MAX_VEL = 39.4224324932042;
-    public static double MAX_ACCEL = 39.4224324932042;
-    public static double MAX_ANG_VEL = Math.toRadians(164.27192727272728);
-    public static double MAX_ANG_ACCEL = Math.toRadians(164.27192727272728);
+    public static double MAX_VEL = 37.01955958930827; // 41.716818064308676;
+    public static double MAX_ACCEL = 41.716818064308676;
+    public static double MAX_ANG_VEL = Math.toRadians(173.83255343999997);
+    public static double MAX_ANG_ACCEL = Math.toRadians(173.83255343999997);
 
 
     public static double encoderTicksToInches(double ticks) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
index 492f466..5c1ebf7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
@@ -5,6 +5,7 @@
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -12,13 +13,14 @@
 import org.firstinspires.ftc.teamcode.slide.LinearSlide;
 
 @Autonomous(name = "Auto Test")
+@Disabled
 public class AutoTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
         StrafeDrive drive = new StrafeDrive(hardwareMap);
-        LinearSlide slide = new LinearSlide(this);
+        LinearSlide slide = new LinearSlide(hardwareMap);
 
         slide.release();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
index 1081e2e..5601055 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
@@ -29,8 +29,8 @@
  * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
  * important as the positional parameters. Like the other manual tuning routines, this op mode
  * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
- * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
- * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
+ * WiFi network. In your browser, navigate to http://192.168.49.1:8080/dash if you're using the RC
+ * phone or http://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
  * connected, start the program, and your robot will begin moving forward and backward according to
  * a motion profile. Your job is to graph the velocity errors over time and adjust the PID
  * coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
index e6d6b23..b7f40bf 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
@@ -12,8 +12,8 @@
  * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
  * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
  * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
- * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
- * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * to the RC's WiFi network. In your browser, navigate to http://192.168.49.1:8080/dash if you're
+ * using the RC phone or http://192.168.43.1:8080/dash if you are using the Control Hub. Once
  * you've successfully connected, start the program, and your robot will begin driving in a square.
  * You should observe the target position (green) and your pose estimate (blue) and adjust your
  * follower PID coefficients such that you follow the target position as accurately as possible.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
index 9b72624..b8d27ab 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
@@ -30,7 +30,7 @@
  * tuning these coefficients is just as important as the positional parameters. Like the other
  * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
  * connect your computer to the RC's WiFi network. In your browser, navigate to
- * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
+ * http://192.168.49.1:8080/dash if you're using the RC phone or http://192.168.43.1:8080/dash if
  * you are using the Control Hub. Once you've successfully connected, start the program, and your
  * robot will begin moving forward and backward according to a motion profile. Your job is to graph
  * the velocity errors over time and adjust the feedforward coefficients. Once you've found a
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
index 932654e..9273b5b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
@@ -2,36 +2,29 @@
 
 import static org.firstinspires.ftc.teamcode.slide.SlideConstants.GRIP_MAX;
 import static org.firstinspires.ftc.teamcode.slide.SlideConstants.GRIP_MIN;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.HOLD_POWER;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.INCREMENT_STEPS;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.LOW_HEIGHT;
 import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MAX_HEIGHT;
 import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MAX_POWER;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MEDIUM_HEIGHT;
 import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MIN_HEIGHT;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MIN_POWER;
 
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.Range;
 
-import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
-
 public class LinearSlide {
-    public DcMotorEx slideMotor;
-    public Servo leftGripper, rightGripper;
+    public final DcMotorEx slideMotor;
+    public final Servo leftGripper, rightGripper;
+    private final HardwareMap hardwareMap;
 
     private boolean gripperClosed = false;
     private boolean dPadPressed = false;
     private int targetArmMotorSteps = 0;
-    private final LinearOpMode opMode;
 
-    public LinearSlide(LinearOpMode opMode) {
-        this.opMode = opMode;
+    public LinearSlide(HardwareMap hardwareMap) {
+        this.hardwareMap = hardwareMap;
 
         // Initialize slide motor
-        slideMotor = opMode.hardwareMap.get(DcMotorEx.class, "motor_slide");
+        slideMotor = hardwareMap.get(DcMotorEx.class, "motor_slide");
         slideMotor.setDirection(DcMotorEx.Direction.REVERSE);
         slideMotor.setTargetPosition(MIN_HEIGHT);
         slideMotor.setPower(MAX_POWER);
@@ -40,105 +33,85 @@ public LinearSlide(LinearOpMode opMode) {
         slideMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
 
         // Initialize gripper servos
-        leftGripper = opMode.hardwareMap.get(Servo.class, "grip1");
-        rightGripper = opMode.hardwareMap.get(Servo.class, "grip2");
+        leftGripper = hardwareMap.get(Servo.class, "grip1");
+        rightGripper = hardwareMap.get(Servo.class, "grip2");
     }
 
-    public void updateTelemetry() {
-        // Add some debug information
-        opMode.telemetry.addData("Target arm motor steps:", targetArmMotorSteps);
-        opMode.telemetry.addData("Actual arm motor steps:", slideMotor.getCurrentPosition());
-        opMode.telemetry.addData("Arm Power:", slideMotor.getPower());
-        opMode.telemetry.addData("Arm Current (A):", slideMotor.getCurrent(CurrentUnit.AMPS));
-        opMode.telemetry.addData("Arm ZeroPower:", slideMotor.getZeroPowerBehavior());
+    public void update() {
+        // Update PID
     }
 
-    public void readGamepad() {
-        // Set arm steps to predefined height with buttons
-        if (opMode.gamepad2.a) {
-            targetArmMotorSteps = MIN_HEIGHT;
-        } else if (opMode.gamepad2.b) {
-            targetArmMotorSteps = LOW_HEIGHT;
-        } else if (opMode.gamepad2.x) {
-            targetArmMotorSteps = MEDIUM_HEIGHT;
-        } else if (opMode.gamepad2.y) {
-            targetArmMotorSteps = MAX_HEIGHT;
-        }
-
-        // Increase arm steps by DPAD increments
-        if (opMode.gamepad2.dpad_up) {
-            targetArmMotorSteps += INCREMENT_STEPS;
-            dPadPressed = true;
-        } else if (opMode.gamepad2.dpad_down) {
-            targetArmMotorSteps -= INCREMENT_STEPS;
-            dPadPressed = true;
-        } else if (dPadPressed) {
-            targetArmMotorSteps = slideMotor.getCurrentPosition();
-            dPadPressed = false;
-        }
-
-        setSlide(targetArmMotorSteps);
-
-        // If motor is busy, run motor at max power.
-        // If motor is not busy, hold at current position or stop at lowest height
-        if (slideMotor.isBusy()) {
-            slideMotor.setPower(MAX_POWER);
-        } else {
-            slideMotor.setPower(HOLD_POWER);
-            if (slideMotor.getCurrentPosition() == MIN_HEIGHT) {
-                slideMotor.setPower(MIN_POWER);
-            }
-        }
-
-        if (opMode.gamepad1.right_trigger > 0.5) {
-            gripperClosed = true;
-        } else if (opMode.gamepad1.left_trigger > 0.5) {
-            gripperClosed = false;
-        }
-
-        if (gripperClosed) {
-            grab();
-        } else {
-            release();
-        }
-    }
-
-    public int getTargetMotorSteps() {
-        return targetArmMotorSteps;
-    }
+//    public void updateTelemetry() {
+//        // Add some debug information
+//        opMode.telemetry.addData("Target arm motor steps:", targetArmMotorSteps);
+//        opMode.telemetry.addData("Actual arm motor steps:", slideMotor.getCurrentPosition());
+//        opMode.telemetry.addData("Arm Power:", slideMotor.getPower());
+//        opMode.telemetry.addData("Arm Current (A):", slideMotor.getCurrent(CurrentUnit.AMPS));
+//        opMode.telemetry.addData("Arm ZeroPower:", slideMotor.getZeroPowerBehavior());
+//    }
+
+//    public void readGamepad() {
+//        // Set arm steps to predefined height with buttons
+//        if (opMode.gamepad2.a) {
+//            targetArmMotorSteps = MIN_HEIGHT;
+//        } else if (opMode.gamepad2.b) {
+//            targetArmMotorSteps = LOW_HEIGHT;
+//        } else if (opMode.gamepad2.x) {
+//            targetArmMotorSteps = MEDIUM_HEIGHT;
+//        } else if (opMode.gamepad2.y) {
+//            targetArmMotorSteps = MAX_HEIGHT;
+//        }
+//
+//        // Increase arm steps by DPAD increments
+//        if (opMode.gamepad2.dpad_up) {
+//            targetArmMotorSteps += INCREMENT_STEPS;
+//            dPadPressed = true;
+//        } else if (opMode.gamepad2.dpad_down) {
+//            targetArmMotorSteps -= INCREMENT_STEPS;
+//            dPadPressed = true;
+//        } else if (dPadPressed) {
+//            targetArmMotorSteps = slideMotor.getCurrentPosition();
+//            dPadPressed = false;
+//        }
+//
+//        setSlide(targetArmMotorSteps);
+//
+//        // If motor is busy, run motor at max power.
+//        // If motor is not busy, hold at current position or stop at lowest height
+//        if (slideMotor.isBusy()) {
+//            slideMotor.setPower(MAX_POWER);
+//        } else {
+//            slideMotor.setPower(HOLD_POWER);
+//            if (slideMotor.getCurrentPosition() == MIN_HEIGHT) {
+//                slideMotor.setPower(MIN_POWER);
+//            }
+//        }
+//
+//        if (opMode.gamepad1.right_trigger > 0.5) {
+//            gripperClosed = true;
+//        } else if (opMode.gamepad1.left_trigger > 0.5) {
+//            gripperClosed = false;
+//        }
+//
+//        if (gripperClosed) {
+//            grab();
+//        } else {
+//            release();
+//        }
+//    }
 
     public void grab() {
         leftGripper.setPosition(GRIP_MAX + 0.1);
         rightGripper.setPosition(GRIP_MIN - 0.1);
     }
 
-    public void grabAndWait() {
-        grab();
-        while (leftGripper.getPosition() < GRIP_MAX || rightGripper.getPosition() > GRIP_MIN) {
-            opMode.idle();
-        }
-        opMode.sleep(300);
-    }
-
     public void release() {
         leftGripper.setPosition(GRIP_MIN);
         rightGripper.setPosition(GRIP_MAX);
     }
 
-    public void releaseAndWait() {
-        release();
-        while (leftGripper.getPosition() > GRIP_MIN || rightGripper.getPosition() < GRIP_MAX) {
-            opMode.idle();
-        }
-    }
-
     public void setSlide(int steps) {
         targetArmMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
         slideMotor.setTargetPosition(targetArmMotorSteps);
     }
-
-    public void setSlideAndWait(int steps) {
-        setSlide(steps);
-        //opMode.blockOn(slideMotor);
-    }
 }

From 2c5b62f5f404b8579f022adc90c561d3a02a166f Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Sun, 19 Feb 2023 21:39:53 -0500
Subject: [PATCH 13/43] fixed right motor order and tuned straight, strafe, and
 turn

---
 .../ftc/teamcode/drive/DriveConstants.java       | 10 +++++-----
 ...{StrafeDrive.java => SampleMecanumDrive.java} | 11 ++++++-----
 .../ftc/teamcode/opmode/AutoTest.java            |  4 ++--
 .../opmode/AutomaticFeedforwardTuner.java        |  4 ++--
 .../ftc/teamcode/opmode/BackAndForth.java        |  4 ++--
 .../teamcode/opmode/DriveVelocityPIDTuner.java   |  4 ++--
 .../ftc/teamcode/opmode/FollowerPIDTuner.java    |  4 ++--
 .../ftc/teamcode/opmode/LocalizationTest.java    |  4 ++--
 .../teamcode/opmode/ManualFeedforwardTuner.java  |  6 +++---
 .../ftc/teamcode/opmode/MaxAngularVeloTuner.java |  4 ++--
 .../ftc/teamcode/opmode/MaxVelocityTuner.java    |  4 ++--
 .../teamcode/opmode/MotorDirectionDebugger.java  |  6 ++----
 .../ftc/teamcode/opmode/SplineTest.java          |  4 ++--
 .../ftc/teamcode/opmode/StrafeTest.java          |  4 ++--
 .../ftc/teamcode/opmode/StraightTest.java        |  4 ++--
 .../ftc/teamcode/opmode/TeleOpTest.java          |  4 ++--
 .../ftc/teamcode/opmode/TrackWidthTuner.java     |  4 ++--
 .../opmode/TrackingWheelForwardOffsetTuner.java  |  4 ++--
 .../TrackingWheelLateralDistanceTuner.java       |  4 ++--
 .../ftc/teamcode/opmode/TurnTest.java            |  4 ++--
 .../ftc/teamcode/util/LogFiles.java              | 16 ++++++++--------
 21 files changed, 56 insertions(+), 57 deletions(-)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/{StrafeDrive.java => SampleMecanumDrive.java} (97%)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
index 6c9d919..477a9bf 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -34,7 +34,7 @@ public class DriveConstants {
      * from DriveVelocityPIDTuner.
      */
     public static final boolean RUN_USING_ENCODER = true;
-    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(30, 0, 12, 12.759179677930504); // f = getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(30, 0, 12, 12.627677180197198); // f = getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
 
     /*
      * These are physical constants that can be determined from your robot (including the track
@@ -45,8 +45,8 @@ public class DriveConstants {
      * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
      */
     public static double WHEEL_RADIUS = 1.4763; // in
-    public static double GEAR_RATIO = 0.05291; // output (wheel) speed / input (motor) speed
-    public static double TRACK_WIDTH = 13.75; // in
+    public static double GEAR_RATIO = 0.05584806; // 0.05291; // output (wheel) speed / input (motor) speed
+    public static double TRACK_WIDTH = 10.81; // 13.75; // in
 
     /*
      * These are the feedforward parameters used to model the drive motor behavior. If you are using
@@ -86,9 +86,9 @@ public class DriveConstants {
      * You are free to raise this on your own if you would like. It is best determined through experimentation.
 
      */
-    public static double MAX_VEL = 37.01955958930827; // 41.716818064308676;
+    public static double MAX_VEL = 38.56039361602114; // 41.716818064308676;
     public static double MAX_ACCEL = 41.716818064308676;
-    public static double MAX_ANG_VEL = Math.toRadians(173.83255343999997);
+    public static double MAX_ANG_VEL = Math.toRadians(142.4500100542844); // 173.83255343999997
     public static double MAX_ANG_ACCEL = Math.toRadians(173.83255343999997);
 
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StrafeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
similarity index 97%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StrafeDrive.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
index 280bdcd..10cc8b6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StrafeDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -53,11 +53,11 @@
  * Simple mecanum drive hardware implementation for REV hardware.
  */
 @Config
-public class StrafeDrive extends MecanumDrive {
+public class SampleMecanumDrive extends MecanumDrive {
     public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
     public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
 
-    public static double LATERAL_MULTIPLIER = 1;
+    public static double LATERAL_MULTIPLIER = 1.0813788830983113; // 1
 
     public static double VX_WEIGHT = 1;
     public static double VY_WEIGHT = 1;
@@ -76,7 +76,7 @@ public class StrafeDrive extends MecanumDrive {
     private IMU imu;
     private VoltageSensor batteryVoltageSensor;
 
-    public StrafeDrive(HardwareMap hardwareMap) {
+    public SampleMecanumDrive(HardwareMap hardwareMap) {
         super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
 
         follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
@@ -96,10 +96,11 @@ public StrafeDrive(HardwareMap hardwareMap) {
                 RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
         imu.initialize(parameters);
 
+
         leftFront = hardwareMap.get(DcMotorEx.class, "motor_front_left");
         leftRear = hardwareMap.get(DcMotorEx.class, "motor_back_left");
-        rightRear = hardwareMap.get(DcMotorEx.class, "motor_back_right");
-        rightFront = hardwareMap.get(DcMotorEx.class, "motor_front_right");
+        rightRear = hardwareMap.get(DcMotorEx.class, "motor_front_right"); // ???
+        rightFront = hardwareMap.get(DcMotorEx.class, "motor_back_right"); // ???
 
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
index 5c1ebf7..069f76e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.slide.LinearSlide;
 
 @Autonomous(name = "Auto Test")
@@ -19,7 +19,7 @@ public class AutoTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
         LinearSlide slide = new LinearSlide(hardwareMap);
 
         slide.release();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
index 5db99bd..d714b78 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
@@ -15,7 +15,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.util.LoggingUtil;
 import org.firstinspires.ftc.teamcode.util.RegressionUtil;
 
@@ -47,7 +47,7 @@ public void runOpMode() throws InterruptedException {
 
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         NanoClock clock = NanoClock.system();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
index 9be133e..807fe6b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
@@ -6,7 +6,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /*
  * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
@@ -32,7 +32,7 @@ public class BackAndForth extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
                 .forward(DISTANCE)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
index 5601055..828eea3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
@@ -20,7 +20,7 @@
 import com.qualcomm.robotcore.util.RobotLog;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 import java.util.List;
 
@@ -73,7 +73,7 @@ public void runOpMode() {
 
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         Mode mode = Mode.TUNING_MODE;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
index b7f40bf..d563e94 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
@@ -5,7 +5,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
 
 /*
@@ -28,7 +28,7 @@ public class FollowerPIDTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
index 1a28584..73ac2eb 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
@@ -5,7 +5,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /**
  * This is a simple teleop routine for testing localization. Drive the robot around like a normal
@@ -18,7 +18,7 @@
 public class LocalizationTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
index b8d27ab..76db64f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
@@ -21,7 +21,7 @@
 import com.qualcomm.robotcore.util.RobotLog;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 import java.util.Objects;
 
@@ -47,7 +47,7 @@ public class ManualFeedforwardTuner extends LinearOpMode {
 
     private FtcDashboard dashboard = FtcDashboard.getInstance();
 
-    private StrafeDrive drive;
+    private SampleMecanumDrive drive;
 
     enum Mode {
         DRIVER_MODE,
@@ -71,7 +71,7 @@ public void runOpMode() {
 
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
 
-        drive = new StrafeDrive(hardwareMap);
+        drive = new SampleMecanumDrive(hardwareMap);
 
         mode = Mode.TUNING_MODE;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
index b6c83dd..79c9df2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
@@ -10,7 +10,7 @@
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 import java.util.Objects;
 
@@ -32,7 +32,7 @@ public class MaxAngularVeloTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
index 54cdbb9..ccc4118 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
@@ -12,7 +12,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 import java.util.Objects;
 
@@ -36,7 +36,7 @@ public class MaxVelocityTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
index da02bb7..e638f91 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
@@ -3,12 +3,11 @@
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /**
  * This is a simple teleop routine for debugging your motor configuration.
@@ -38,7 +37,6 @@
  * <p>
  * Uncomment the @Disabled tag below to use this opmode.
  */
-@Disabled
 @Config
 @TeleOp(group = "drive")
 public class MotorDirectionDebugger extends LinearOpMode {
@@ -48,7 +46,7 @@ public class MotorDirectionDebugger extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         telemetry.addLine("Press play to begin the debugging opmode");
         telemetry.update();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
index 8c4de46..506c3b3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
@@ -6,7 +6,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /*
  * This is an example of a more complex path to really test the tuning.
@@ -15,7 +15,7 @@
 public class SplineTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         waitForStart();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
index 07689b0..90ff4d7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /*
  * This is a simple routine to test translational drive capabilities.
@@ -23,7 +23,7 @@ public class StrafeTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
                 .strafeRight(DISTANCE)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
index d0c0648..bee34af 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /*
  * This is a simple routine to test translational drive capabilities.
@@ -23,7 +23,7 @@ public class StraightTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
                 .forward(DISTANCE)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
index e887970..fb5bfd4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
@@ -7,7 +7,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 @TeleOp(name = "TeleOp Test")
 public class TeleOpTest extends LinearOpMode {
@@ -15,7 +15,7 @@ public class TeleOpTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         waitForStart();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
index 4739c65..b852d3b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
@@ -12,7 +12,7 @@
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
 import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /*
  * This routine determines the effective track width. The procedure works by executing a point turn
@@ -34,7 +34,7 @@ public class TrackWidthTuner extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
         // TODO: if you haven't already, set the localizer to something that doesn't depend on
         // drive encoders for computing the heading
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
index a129da7..8be29de 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
@@ -12,8 +12,8 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /**
  * This routine determines the effective forward offset for the lateral tracking wheel.
@@ -45,7 +45,7 @@ public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
             RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
index 5e1ef54..22c8224 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
@@ -7,8 +7,8 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.util.RobotLog;
 
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 /**
  * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
@@ -68,7 +68,7 @@ public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
             RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
index 682df14..562b415 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
@@ -4,7 +4,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 
 /*
  * This is a simple routine to test turning capabilities.
@@ -16,7 +16,7 @@ public class TurnTest extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        StrafeDrive drive = new StrafeDrive(hardwareMap);
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         waitForStart();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
index 7b2ddab..66e5e9d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
@@ -16,9 +16,9 @@
 import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
 import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
 import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.drive.SampleTankDrive;
 import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
-import org.firstinspires.ftc.teamcode.drive.StrafeDrive;
 
 import java.io.File;
 import java.io.FileInputStream;
@@ -65,13 +65,13 @@ public static class LogFile {
         public double maxAngVel = DriveConstants.MAX_ANG_VEL;
         public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL;
 
-        public double mecTransP = StrafeDrive.TRANSLATIONAL_PID.kP;
-        public double mecTransI = StrafeDrive.TRANSLATIONAL_PID.kI;
-        public double mecTransD = StrafeDrive.TRANSLATIONAL_PID.kD;
-        public double mecHeadingP = StrafeDrive.HEADING_PID.kP;
-        public double mecHeadingI = StrafeDrive.HEADING_PID.kI;
-        public double mecHeadingD = StrafeDrive.HEADING_PID.kD;
-        public double mecLateralMultiplier = StrafeDrive.LATERAL_MULTIPLIER;
+        public double mecTransP = SampleMecanumDrive.TRANSLATIONAL_PID.kP;
+        public double mecTransI = SampleMecanumDrive.TRANSLATIONAL_PID.kI;
+        public double mecTransD = SampleMecanumDrive.TRANSLATIONAL_PID.kD;
+        public double mecHeadingP = SampleMecanumDrive.HEADING_PID.kP;
+        public double mecHeadingI = SampleMecanumDrive.HEADING_PID.kI;
+        public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD;
+        public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER;
 
         public double tankAxialP = SampleTankDrive.AXIAL_PID.kP;
         public double tankAxialI = SampleTankDrive.AXIAL_PID.kI;

From 454312595d71385bec9ef607bfd4e4f8ea91ad6f Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Mon, 20 Feb 2023 15:20:19 -0500
Subject: [PATCH 14/43] tuned PIDs and track width

---
 .../ftc/teamcode/drive/DriveConstants.java    |  4 +--
 .../teamcode/drive/SampleMecanumDrive.java    |  4 +--
 .../ftc/teamcode/opmode/AutoTest.java         | 20 +++----------
 .../ftc/teamcode/opmode/SplineTest.java       | 29 ++++++++++++-------
 4 files changed, 27 insertions(+), 30 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
index 477a9bf..1d6b44b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -34,7 +34,7 @@ public class DriveConstants {
      * from DriveVelocityPIDTuner.
      */
     public static final boolean RUN_USING_ENCODER = true;
-    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(30, 0, 12, 12.627677180197198); // f = getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(20, 0, 5, 12.627677180197198); // f = getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); // p=30, d=12
 
     /*
      * These are physical constants that can be determined from your robot (including the track
@@ -46,7 +46,7 @@ public class DriveConstants {
      */
     public static double WHEEL_RADIUS = 1.4763; // in
     public static double GEAR_RATIO = 0.05584806; // 0.05291; // output (wheel) speed / input (motor) speed
-    public static double TRACK_WIDTH = 10.81; // 13.75; // in
+    public static double TRACK_WIDTH = 9.58; // 10.56; // 10.81; // 13.75; // in
 
     /*
      * These are the feedforward parameters used to model the drive motor behavior. If you are using
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
index 10cc8b6..e9e9b1d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -54,8 +54,8 @@
  */
 @Config
 public class SampleMecanumDrive extends MecanumDrive {
-    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
-    public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 0);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(3, 0, 0);
 
     public static double LATERAL_MULTIPLIER = 1.0813788830983113; // 1
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
index 069f76e..8a66dd0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
@@ -3,14 +3,13 @@
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.slide.LinearSlide;
 
 @Autonomous(name = "Auto Test")
 @Disabled
@@ -20,24 +19,13 @@ public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
 
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-        LinearSlide slide = new LinearSlide(hardwareMap);
-
-        slide.release();
 
         waitForStart();
 
         if (isStopRequested()) return;
 
-        // First Cone
-        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
-                //.addDisplacementMarker(slide::grab)
-                .forward(5.5)
-                .strafeLeft(37.5)
-                //.addDisplacementMarker(() -> slide.setSlideAndWait(1900))
-                .forward(6.5)
-                //.addDisplacementMarker(slide::release)
-                .build();
-
-        drive.followTrajectory(trajectory);
+        drive.followTrajectory(drive.trajectoryBuilder(new Pose2d())
+                .splineTo(new Vector2d(30, 30), 0)
+                .build());
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
index 506c3b3..138771e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmode;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.acmerobotics.roadrunner.trajectory.Trajectory;
@@ -11,8 +12,12 @@
 /*
  * This is an example of a more complex path to really test the tuning.
  */
+@Config
 @Autonomous(group = "drive")
 public class SplineTest extends LinearOpMode {
+    public static int TRIALS = 1;
+    public static int DELAY = 2000;
+
     @Override
     public void runOpMode() throws InterruptedException {
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@@ -21,18 +26,22 @@ public void runOpMode() throws InterruptedException {
 
         if (isStopRequested()) return;
 
-        Trajectory traj = drive.trajectoryBuilder(new Pose2d())
-                .splineTo(new Vector2d(30, 30), 0)
-                .build();
+        for (int i = 0; i < TRIALS; i++) {
+            Trajectory traj = drive.trajectoryBuilder(new Pose2d())
+                    .splineTo(new Vector2d(30, 30), 0)
+                    .build();
+
+            drive.followTrajectory(traj);
 
-        drive.followTrajectory(traj);
+            sleep(DELAY);
 
-        sleep(2000);
+            drive.followTrajectory(
+                    drive.trajectoryBuilder(traj.end(), true)
+                            .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+                            .build()
+            );
 
-        drive.followTrajectory(
-                drive.trajectoryBuilder(traj.end(), true)
-                        .splineTo(new Vector2d(0, 0), Math.toRadians(180))
-                        .build()
-        );
+            sleep(DELAY);
+        }
     }
 }

From 2eedded85743826d76d8a3a9be55377e160d3df3 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Mon, 20 Feb 2023 20:04:44 -0500
Subject: [PATCH 15/43] reorganized directory structure

---
 .../teamcode/{opmode => auto}/AutoTest.java   |   4 +-
 .../ftc/teamcode/old/RobotOpMode.java         |  30 --
 .../ftc/teamcode/old/auto/AutoRight.java      | 173 ------
 .../ftc/teamcode/old/auto/AutoTestRight.java  | 195 -------
 .../teamcode/old/pipeline/BoeDetection.java   | 185 -------
 .../teamcode/old/pipeline/PoleDetection.java  | 103 ----
 .../old/pipeline/SleeveDetectionNew.java      |  89 ---
 .../old/pipeline/SleeveDetectionRight.java    | 144 -----
 .../teamcode/old/subsystem/LinearSlide.java   | 144 -----
 .../teamcode/old/subsystem/StrafeDrive.java   | 510 ------------------
 .../ftc/teamcode/old/subsystem/Subsystem.java |  13 -
 .../ftc/teamcode/old/subsystem/Webcam.java    |  40 --
 .../ftc/teamcode/old/teleop/SensorColor.java  | 226 --------
 .../ftc/teamcode/slide/LinearSlide.java       | 117 ----
 .../ftc/teamcode/slide/SlideConstants.java    |  36 --
 .../{ => subsystem}/drive/DriveConstants.java |   2 +-
 .../drive/SampleMecanumDrive.java             |  26 +-
 .../drive/SampleTankDrive.java                |  26 +-
 .../drive/StandardTrackingWheelLocalizer.java |   3 +-
 .../{opmode => teleop}/TeleOpTest.java        |   4 +-
 .../AutomaticFeedforwardTuner.java            |  10 +-
 .../{opmode => tuning}/BackAndForth.java      |   4 +-
 .../DriveVelocityPIDTuner.java                |  14 +-
 .../{opmode => tuning}/FollowerPIDTuner.java  |   4 +-
 .../{opmode => tuning}/LocalizationTest.java  |   4 +-
 .../ManualFeedforwardTuner.java               |  16 +-
 .../MaxAngularVeloTuner.java                  |   4 +-
 .../{opmode => tuning}/MaxVelocityTuner.java  |   6 +-
 .../MotorDirectionDebugger.java               |   4 +-
 .../{opmode => tuning}/SplineTest.java        |   4 +-
 .../{opmode => tuning}/StrafeTest.java        |   4 +-
 .../{opmode => tuning}/StraightTest.java      |   4 +-
 .../{opmode => tuning}/TrackWidthTuner.java   |   6 +-
 .../TrackingWheelForwardOffsetTuner.java      |   6 +-
 .../TrackingWheelLateralDistanceTuner.java    |   6 +-
 .../teamcode/{opmode => tuning}/TurnTest.java |   4 +-
 .../ftc/teamcode/util/LogFiles.java           |   8 +-
 37 files changed, 87 insertions(+), 2091 deletions(-)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => auto}/AutoTest.java (89%)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNew.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRight.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => subsystem}/drive/DriveConstants.java (99%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => subsystem}/drive/SampleMecanumDrive.java (91%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => subsystem}/drive/SampleTankDrive.java (91%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => subsystem}/drive/StandardTrackingWheelLocalizer.java (98%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => teleop}/TeleOpTest.java (96%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/AutomaticFeedforwardTuner.java (95%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/BackAndForth.java (94%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/DriveVelocityPIDTuner.java (92%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/FollowerPIDTuner.java (95%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/LocalizationTest.java (93%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/ManualFeedforwardTuner.java (90%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/MaxAngularVeloTuner.java (95%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/MaxVelocityTuner.java (94%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/MotorDirectionDebugger.java (96%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/SplineTest.java (91%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/StrafeTest.java (92%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/StraightTest.java (92%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/TrackWidthTuner.java (94%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/TrackingWheelForwardOffsetTuner.java (95%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/TrackingWheelLateralDistanceTuner.java (96%)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{opmode => tuning}/TurnTest.java (83%)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
similarity index 89%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
index 8a66dd0..d1b00bd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.auto;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 @Autonomous(name = "Auto Test")
 @Disabled
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
deleted file mode 100644
index 255ac0c..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.firstinspires.ftc.teamcode.old;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.subsystem.Subsystem;
-
-abstract public class RobotOpMode extends LinearOpMode {
-    public void addSubsystems(Subsystem... subsystems) {
-        for (Subsystem subsystem : subsystems) {
-            subsystem.setup();
-        }
-    }
-
-    // Waits until at least one device or OpMode is not busy
-    public void blockOn(DcMotor... motors) {
-        while (opModeIsActive() && isBusy(motors)) {
-            idle();
-        }
-    }
-
-    public boolean isBusy(DcMotor... motors) {
-        for (DcMotor motor : motors) {
-            if (!motor.isBusy()) {
-                return false;
-            }
-        }
-        return true;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
deleted file mode 100644
index eb16ea1..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
+++ /dev/null
@@ -1,173 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.RobotOpMode;
-import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRight;
-import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRight.ParkingPosition;
-import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
-import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
-import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
-
-@Autonomous(name = "Autonomous Right")
-public class AutoRight extends RobotOpMode {
-    private final StrafeDrive drive = new StrafeDrive(this);
-    private final LinearSlide slide = new LinearSlide(this);
-    private final SleeveDetectionRight sleeve = new SleeveDetectionRight();
-    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
-
-    @Override
-    public void runOpMode() {
-        addSubsystems(drive, slide, cam);
-
-        // Put motors in encoder mode
-        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        slide.release();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-        ParkingPosition position = sleeve.getPosition();
-        //Place First Cone
-        slide.grab();
-        drive.driveInches(5.5, 5.5, 0.2f);
-        slide.setSlideAndWait(800);
-        drive.strafeInches(-37.5, 0.5f);
-        slide.setSlideAndWait(1900);
-        drive.driveInches(6.5, 6.5, 0.2f);
-        slide.release();
-
-        //Go for Second Cone
-        drive.strafeInches(-14.0, 0.5f);
-        slide.setSlideAndWait(500);
-        drive.driveInches(20, 20, 0.5f);
-        slide.grab();
-        slide.setSlideAndWait(1000);
-        drive.driveInches(-1, -1, 0.25f);
-        slide.setSlideAndWait(1500);
-        drive.driveInches(-5, -5, 0.25f);
-        slide.setSlideAndWait(300);
-        drive.driveInches(-44, -44, 0.5f);
-        drive.strafeInches(12.5, 0.5f);
-        slide.setSlideAndWait(3200);
-        drive.driveInches(4, 4, 0.2f);
-        slide.release();
-        drive.driveInches(-4, -4, 0.2f);
-        slide.setSlideAndWait(280);
-        drive.driveInches(0, 1.5, 0.5f);
-        drive.strafeInches(-12, 0.5f);
-
-
-        //Third Cone
-//        drive.driveInches(50, 50, 0.3f);
-//        slide.grab();
-//        slide.setSlide(150, true);
-//        drive.driveInches(-1, -1, 0.25f);
-//        slide.setSlide(375, true);
-//        drive.driveInches(-5, -5, 0.25f);
-//        slide.setSlide(50, true);
-//        drive.driveInches(-38, -38, 0.5f);
-//        slide.setSlide(1000, true);
-//        drive.drive(700, -700, 0.2f);
-//        drive.driveInches(4, 4, 0.2f);
-//        slide.ungrab();
-//        drive.driveInches(-4, -4, 0.2f);
-//        drive.drive(-700, 700, 0.2f);
-
-        drive.driveInches(20, 20, 0.5f);
-        slide.grab();
-        slide.setSlideAndWait(250);
-        drive.driveInches(-1, -1, 0.25f);
-        slide.setSlideAndWait(375);
-        drive.driveInches(-5, -5, 0.25f);
-        slide.setSlideAndWait(75);
-        drive.driveInches(-44, -44, 0.5f);
-        drive.strafeInches(12.5, 0.5f);
-        slide.setSlideAndWait(800);
-        drive.driveInches(4, 4, 0.2f);
-        slide.release();
-        drive.driveInches(-4, -4, 0.2f);
-        slide.setSlideAndWait(70);
-        drive.driveInches(0, 1.5, 0.5f);
-        drive.strafeInches(-12, 0.5f);
-
-
-        //Move to Parking
-        switch (position) {
-
-            case CENTER:
-                drive.driveInches(24, 24, 0.2f);
-                break;
-            case RIGHT:
-                drive.driveInches(48, 48, 0.2f);
-                break;
-            case LEFT:
-                break;
-        }
-        slide.setSlideAndWait(0);
-        /*
-        1350 is Approximately 25 inches moving forwards
-        1350 is 24 inches moving sideways
-        270 rotation is approximately 35 degrees
-        250 linear slide is 1.5 inches
-
-        Directions
-        strafe 39.5 inches
-        place cone
-        strafe 12 inches
-        rotate 180 degrees
-        move forwards 27 inches
-        */
-
-        //        robot.linearSlide.grab();
-//        drive.driveInches(3, 3, 0.25f);
-//        drive.strafeInches(39.5, 0.25f);
-
-//        drive.drive(200, 200, 0.25f);
-
-        //place first cone (low)
-
-//        robot.linearSlide.unGrab();
-
-//        drive.drive(-200, -200, 0.25f);zs
-//        slide.setSlide(0);
-//        drive.strafeInches(20, 0.25f);
-//        robot.strafeDrive.drive(-926, 926, 0.5f);
-//        drive.driveInches(27, 27, 0.25f);
-        //grab second cone
-//        slide.setSlide(300);
-
-//        robot.linearSlide.grab();
-
-//        slide.setSlide(450);
-//        drive.driveInches(-3, -3, 0.25f);
-//        slide.setSlide(50);
-
-//        drive.driveInches(-37, -37, 0.25f);
-//        drive.strafeInches(-10, 0.5f);
-//        drive.drive(200, 200, 0.5f);
-        //place second cone (mid)
-
-//        slide.setSlide(300);
-//        drive.drive(-200, -200, 0.5f);
-//        drive.strafeInches(10, 0.5f);
-//        drive.driveInches(40, 40, 0.5f);
-
-        //grab third cone
-
-//        drive.driveInches(-35, -35, 0.5f);
-//        drive.drive(-463, 463, 0.5f);
-
-//        place third cone
-
-//        drive.drive(463, -463, 0.5f);
-        //park
-
-//        robot.strafeDrive.drive(1680, 1680, 0.5f);
-//        robot.strafeDrive.strafe(1000, 0.5f);
-//        robot.strafeDrive.strafe(-1000, 0.5f);
-//        robot.strafeDrive.drive(1000, -1000, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, -0.5f);
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
deleted file mode 100644
index cbdf326..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
+++ /dev/null
@@ -1,195 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.RobotOpMode;
-import org.firstinspires.ftc.teamcode.old.pipeline.PoleDetection;
-import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNew;
-import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNew.ParkingPosition;
-import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
-import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
-import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
-
-@Autonomous(name = "Autonomous Test Right")
-public class AutoTestRight extends RobotOpMode {
-    private final StrafeDrive drive = new StrafeDrive(this);
-    private final LinearSlide slide = new LinearSlide(this);
-    private final SleeveDetectionNew sleeve = new SleeveDetectionNew();
-    private final PoleDetection pole = new PoleDetection();
-    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
-
-    @Override
-    public void runOpMode() {
-        addSubsystems(drive, slide, cam);
-
-        // Put motors in encoder mode
-        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        slide.release();
-
-        drive.imu.resetYaw();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-        drive.imu.resetYaw();
-        ParkingPosition position = sleeve.getPosition();
-        slide.grab();
-        slide.setSlide(200);
-        drive.driveInches(2, 2, 0.75f);
-//        drive.drive(50,-50, 0.75f);
-        drive.strafeInches(-20, 0.6f);
-        drive.driveInches(55, 55, 0.75f);
-//        drive.strafeInches(-12, 0.5f);
-        slide.setSlideAndWait(4000);
-        drive.drive(375, -375, 0.35f);
-        drive.driveInches(11, 11, 0.2f);
-        drive.driveInches(-4, -4, 0.2f);
-        slide.setSlideAndWait(3250);
-        slide.release();
-        slide.setSlideAndWait(4000);
-        drive.driveInches(-5, -5, 0.2f);
-        slide.setSlide(600);
-//        drive.driveInches(-2250,-2250, 0.5f);
-        //Second CONEEeee
-        drive.drive(450, -450, 0.35f);
-        drive.strafeInches(2, 0.6f);
-        drive.driveInches(48, 48, 0.75f);
-//        drive.strafeInches(4, 0.5f);
-//        drive.driveInches(10,10,0.25f);
-        drive.driveInches(-2, -2, 0.5f);
-        slide.grab();
-        slide.setSlideAndWait(1400);
-        drive.driveInches(-5, -5, 0.25f);
-        drive.drive(50, -50, 0.25f);
-//        drive.strafeInches(-4,0.25f);
-        slide.setSlide(460);
-        drive.driveInches(-16, -16, 0.75f);
-        drive.driveInches(-16, -16, 0.75f);
-        drive.drive(-850, 850, 0.25f);
-        slide.setSlideAndWait(4000);
-        drive.driveInches(8, 8, 0.2f);
-        drive.driveInches(-2, -2, 0.2f);
-        slide.setSlideAndWait(3250);
-        slide.release();
-        slide.setSlideAndWait(4000);
-        drive.driveInches(-3, -3, 0.2f);
-        slide.setSlideAndWait(0);
-        //Stuff Work
-
-        //THIRD CONEEEEE
-//        slide.setSlide(300,true);
-//        drive.drive(-725,725,0.35f);
-//        drive.driveInches(21,21,0.5f);
-//        drive.driveInches(16,16,0.5f);
-//        slide.grab();
-//        drive.driveInches(2,2,0.35f);
-//        slide.setSlide(1500,true);
-//        drive.driveInches(-3,-3,0.25f);
-//        slide.setSlide(420,true);
-//        drive.strafeInches(1,0.35f);
-//        drive.driveInches(-32,-32,0.75f);
-//        drive.drive(725,-725,0.25f);
-//        slide.setSlide(4000, true);
-//        drive.driveInches(2,2, 0.2f);
-//        slide.setSlide(3250, true);
-//        slide.ungrab();
-//        drive.driveInches(-2,-2,0.2f);
-//        slide.setSlide(0, true);
-
-        //GAS GAS GAS
-//        drive.driveInches(2,2,1f);
-//        drive.strafeInches(18,0.75f);
-////        drive.turnRight(0, 0.2f);
-//        drive.driveInches(50, 50,0.75f);
-//        drive.strafeInches(-12, 0.5f);
-////        drive.turnRight(0, 0.2f);
-////        cam.camera.setPipeline(pole);
-////        while(!pole.getPole()) {
-////            drive.strafeInches(-2, 0.5f);
-////        }
-//        slide.setSlide(4000, true);
-//        drive.driveInches(4,4, 0.2f);
-//        slide.setSlide(3250, true);
-//        slide.ungrab();
-//        drive.driveInches(-5,-5,0.2f);
-//        slide.setSlide(600, true);
-//
-////        drive.driveInches(-2250,-2250, 0.5f);
-//        //Second CONEEeee
-//        drive.drive(-775,775,0.35f);
-//        drive.driveInches(21,21,1f);
-//        drive.strafeInches(4, 0.5f);
-//        drive.driveInches(16,16,0.5f);
-//        slide.grab();
-//        drive.driveInches(5,5,0.2f);
-//        slide.setSlide(1500,true);
-//        drive.driveInches(-5,-5,0.25f);
-//        slide.setSlide(460,true);
-//        drive.strafeInches(2,0.35f);
-//        drive.driveInches(-16,-16,0.75f);
-//        drive.strafeInches(-2,1f);
-//        drive.driveInches(-16,-16,0.75f);
-//        drive.drive(725,-725,0.25f);
-//        slide.setSlide(4000, true);
-//        drive.driveInches(1,1, 0.2f);
-//        slide.setSlide(3250, true);
-//        slide.ungrab();
-//        drive.driveInches(-3,-3,0.2f);
-//        //Stuff Work
-//
-//        //THIRD CONEEEEE
-////        slide.setSlide(300,true);
-////        drive.drive(-725,725,0.35f);
-////        drive.driveInches(21,21,0.5f);
-////        drive.driveInches(16,16,0.5f);
-////        slide.grab();
-////        drive.driveInches(2,2,0.35f);
-////        slide.setSlide(1500,true);
-////        drive.driveInches(-3,-3,0.25f);
-////        slide.setSlide(420,true);
-////        drive.strafeInches(1,0.35f);
-////        drive.driveInches(-32,-32,0.75f);
-////        drive.drive(725,-725,0.25f);
-////        slide.setSlide(4000, true);
-////        drive.driveInches(2,2, 0.2f);
-////        slide.setSlide(3250, true);
-////        slide.ungrab();
-////        drive.driveInches(-2,-2,0.2f);
-////        slide.setSlide(0, true);
-
-        //Stuff No Work Yet
-        switch (position) {
-            case CENTER:
-                drive.strafeInches(14, 0.2f);
-                break;
-            case RIGHT:
-                drive.strafeInches(38, 0.2f);
-                break;
-            case LEFT:
-                drive.strafeInches(-12, 0.2f);
-                break;
-        }
-
-//        cycle(700);
-    }
-
-    public void cycle(int height) {
-        drive.drive(-850, 850, 0.25f);
-        drive.driveInches(33, 33, 0.45f);
-        slide.setSlideAndWait(height);
-        drive.driveInches(4, 4, 0.25f);
-        slide.grab();
-        drive.driveInches(-1, -1, 0.15f);
-        slide.setSlideAndWait(height + 600);
-//        drive.driveInches(-2, -2,0.25f);
-//        slide.setSlide(900,true);
-        drive.driveInches(-13, -10, 0.25f);
-        slide.setSlideAndWait(300);
-        drive.driveInches(-25, -25, 0.45f);
-        drive.drive(875, -875, 0.25f);
-        slide.setSlideAndWait(4150);
-        drive.driveInches(2, 2, 0.15f);
-        slide.release();
-        drive.driveInches(-2, -2, 0.15f);
-        slide.setSlideAndWait(0);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetection.java
deleted file mode 100644
index 9c8342d..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetection.java
+++ /dev/null
@@ -1,185 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.pipeline;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class BoeDetection extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum Cone {
-        RED,
-        BLUE,
-        UNDECIDED
-    }
-
-    public enum Distance {
-        CLOSE,
-        MID,
-        FAR,
-        UNSEEN
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static final int X = 55;
-    private static final int Y = 120;
-    private static final int STEP = 10;
-    private static final int WIDTH = 25;
-    private static final int HEIGHT = 15;
-    private static final Point FAR_TOPLEFT_ANCHOR_POINT = new Point(X, Y);
-    private static final Point CLOSE_TOPLEFT_ANCHOR_POINT = new Point(X - (2 * STEP), Y - (2 * STEP));
-
-    // Width and height for the bounding box
-    public static int FAR_REGION_WIDTH = WIDTH;
-    public static int FAR_REGION_HEIGHT = HEIGHT;
-    public static int MID_REGION_WIDTH = WIDTH + STEP + STEP;
-    public static int MID_REGION_HEIGHT = HEIGHT + STEP + STEP;
-    public static int CLOSE_REGION_WIDTH = WIDTH + STEP + STEP + STEP + STEP;
-    public static int CLOSE_REGION_HEIGHT = HEIGHT + STEP + STEP + STEP + STEP;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_red_bounds = new Scalar(100, 0, 0, 255),
-            upper_red_bounds = new Scalar(255, 150, 150, 255),
-            lower_blue_bounds = new Scalar(0, 0, 100, 255),
-            upper_blue_bounds = new Scalar(150, 150, 255, 255);
-
-    // Color definitions
-    private final Scalar
-            RED = new Scalar(255, 0, 0),
-            BLUE = new Scalar(0, 0, 255),
-            BLACK = new Scalar(0, 0, 0);
-
-    // Percent and mat definitions
-    private double redPercentC, bluePercentC, redPercentM, bluePercentM, redPercentF, bluePercentF;
-    private final Mat redMatC = new Mat();
-    private final Mat blueMatC = new Mat();
-    private Mat redMatF = new Mat();
-    private Mat blueMatF = new Mat();
-    private Mat redMatM = new Mat();
-    private Mat blueMatM = new Mat();
-    private Mat blurredMatClose = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point((2 * STEP), (2 * STEP));
-    Point sleeve_pointB = new Point(
-            (2 * STEP) + FAR_REGION_WIDTH,
-            (2 * STEP) + FAR_REGION_HEIGHT);
-
-    Point sleeve_pointC = new Point(STEP, STEP);
-    Point sleeve_pointD = new Point(
-            STEP + MID_REGION_WIDTH,
-            STEP + MID_REGION_HEIGHT);
-
-    Point sleeve_pointE = new Point(
-            CLOSE_TOPLEFT_ANCHOR_POINT.x,
-            CLOSE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointF = new Point(
-            CLOSE_TOPLEFT_ANCHOR_POINT.x + CLOSE_REGION_WIDTH,
-            CLOSE_TOPLEFT_ANCHOR_POINT.y + CLOSE_REGION_HEIGHT);
-
-    // Running variable storing the parking position
-    private volatile Cone cone = Cone.UNDECIDED;
-    private volatile Distance distance = Distance.UNSEEN;
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMatClose, new Size(5, 5));
-        blurredMatClose = blurredMatClose.submat(new Rect(sleeve_pointE, sleeve_pointF));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMatClose, blurredMatClose, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMatClose, lower_red_bounds, upper_red_bounds, redMatC);
-        Core.inRange(blurredMatClose, lower_blue_bounds, upper_blue_bounds, blueMatC);
-
-        redMatM = redMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
-        blueMatM = blueMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
-
-        redMatF = redMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
-        blueMatF = blueMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-
-        // Gets color specific values
-        redPercentC = Core.countNonZero(redMatC);
-        bluePercentC = Core.countNonZero(blueMatC);
-
-        redPercentM = Core.countNonZero(redMatM);
-        bluePercentM = Core.countNonZero(blueMatM);
-
-        redPercentF = Core.countNonZero(redMatF);
-        bluePercentF = Core.countNonZero(blueMatF);
-
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(redPercentF, bluePercentF);
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == redPercentF) {
-            if (redPercentC >= 150) {
-                cone = Cone.RED;
-                distance = Distance.CLOSE;
-            } else if (redPercentM >= 100) {
-                cone = Cone.RED;
-                distance = Distance.MID;
-            } else if (redPercentF >= 50) {
-                cone = Cone.RED;
-                distance = Distance.FAR;
-            } else {
-                cone = Cone.UNDECIDED;
-                distance = Distance.UNSEEN;
-            }
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    RED,
-                    2
-            );
-        } else if (maxPercent == bluePercentC && bluePercentC >= 50) {
-            cone = Cone.BLUE;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    BLUE,
-                    2
-            );
-        } else {
-            cone = Cone.UNDECIDED;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    BLACK,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        redMatC.release();
-        blueMatC.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public Cone getPosition() {
-        return cone;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetection.java
deleted file mode 100644
index 4070cd2..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetection.java
+++ /dev/null
@@ -1,103 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.pipeline;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class PoleDetection extends OpenCvPipeline {
-    private volatile boolean isPole = false;
-
-    // TOPLEFT anchor point for the bounding box
-    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(50, 85);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 50;
-    public static int REGION_HEIGHT = 30;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds = new Scalar(255, 255, 150, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255);
-
-    // Percent and mat definitions
-    private double yelPercent;
-    private final Mat yelMat = new Mat();
-    private Mat blurredMat = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-//
-//        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-//        Core.inRange(input, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-
-        // Gets color specific values
-//        Imgproc.Canny(yelMat, yelMat, 100, 200);
-        yelPercent = Core.countNonZero(yelMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (yelPercent >= 100) {
-            isPole = true;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else {
-            isPole = false;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    public boolean getPole() {
-        return isPole;
-    }
-
-    public double getPercent() {
-        return yelPercent;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNew.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNew.java
deleted file mode 100644
index b17ebe3..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNew.java
+++ /dev/null
@@ -1,89 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.pipeline;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class SleeveDetectionNew extends OpenCvPipeline {
-
-    public enum ParkingPosition {
-        LEFT,
-        CENTER,
-        RIGHT
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(65, 50);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Color definitions
-    private final Scalar
-            YELLOW = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255),
-            MAGENTA = new Scalar(255, 0, 255);
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-    // Running variable storing the parking position
-    private volatile ParkingPosition position = ParkingPosition.LEFT;
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Get the submat frame, and then sum all the values
-        Mat areaMat = input.submat(new Rect(sleeve_pointA, sleeve_pointB));
-        Scalar sumColors = Core.sumElems(areaMat);
-
-        double minColor = Math.min(sumColors.val[0], Math.min(sumColors.val[1], sumColors.val[2]));
-
-        if (sumColors.val[0] == minColor) {
-            position = ParkingPosition.CENTER;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        } else if (sumColors.val[1] == minColor) {
-            position = ParkingPosition.RIGHT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    MAGENTA,
-                    2
-            );
-        } else {
-            position = ParkingPosition.LEFT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        }
-
-        // Release and return input
-        areaMat.release();
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public ParkingPosition getPosition() {
-        return position;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRight.java
deleted file mode 100644
index 0a5d7d9..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRight.java
+++ /dev/null
@@ -1,144 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.pipeline;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class SleeveDetectionRight extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum ParkingPosition {
-        LEFT,
-        CENTER,
-        RIGHT
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(125, 175);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-//            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
-//            upper_yellow_bounds = new Scalar(255, 255, 150, 255),
-//            lower_cyan_bounds = new Scalar(0, 120, 120, 255),
-//            upper_cyan_bounds = new Scalar(150, 255, 255, 255),
-//            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
-//            upper_magenta_bounds = new Scalar(255, 170, 255, 255);
-
-            lower_yellow_bounds = new Scalar(120, 120, 0, 255),
-            upper_yellow_bounds = new Scalar(255, 255, 70, 255),
-            lower_cyan_bounds = new Scalar(2, 82, 176, 255),
-            upper_cyan_bounds = new Scalar(48, 255, 255, 255),
-            lower_magenta_bounds = new Scalar(125, 3, 176, 255),
-            upper_magenta_bounds = new Scalar(255, 43, 255, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255),
-            MAGENTA = new Scalar(255, 0, 255);
-
-    // Percent and mat definitions
-    private double yelPercent, cyaPercent, magPercent;
-    private final Mat yelMat = new Mat();
-    private final Mat cyaMat = new Mat();
-    private final Mat magMat = new Mat();
-    private Mat blurredMat = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-    // Running variable storing the parking position
-    private volatile ParkingPosition position = ParkingPosition.LEFT;
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-        Imgproc.cvtColor(input, blurredMat, Imgproc.COLOR_RGB2HSV);
-        blurredMat.convertTo(blurredMat, -1, 1, 10);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-        Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
-        Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
-
-        // Gets color specific values
-        yelPercent = Core.countNonZero(yelMat);
-        cyaPercent = Core.countNonZero(cyaMat);
-        magPercent = Core.countNonZero(magMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == yelPercent) {
-            position = ParkingPosition.LEFT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else if (maxPercent == cyaPercent) {
-            position = ParkingPosition.CENTER;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        } else if (maxPercent == magPercent) {
-            position = ParkingPosition.RIGHT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    MAGENTA,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        cyaMat.release();
-        magMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public ParkingPosition getPosition() {
-        return position;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
deleted file mode 100644
index 127f4f1..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
+++ /dev/null
@@ -1,144 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.subsystem;
-
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.util.Range;
-
-import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
-import org.firstinspires.ftc.teamcode.old.RobotOpMode;
-
-public class LinearSlide extends Subsystem {
-    // Useful Constants
-    public static final double MIN_POWER = 0;
-    public static final double HOLD_POWER = 0.1;
-    public static final double MAX_POWER = 1;
-    public static final int MIN_HEIGHT = 0;
-    public static final int LOW_HEIGHT = 1600;
-    public static final int MEDIUM_HEIGHT = 2760;
-    public static final int MAX_HEIGHT = 3870;
-    public static final int INCREMENT_STEPS = 12;
-    public static final double GRIP_MIN = 0;
-    public static final double GRIP_MAX = 0.6;
-
-    // Hardware
-    public DcMotorEx slideMotor;
-    public Servo leftGripper, rightGripper;
-
-    private boolean gripperClosed = false;
-    private boolean dPadPressed = false;
-    private int armMotorSteps = 0;
-
-    public LinearSlide(RobotOpMode opMode) {
-        super(opMode);
-    }
-
-    @Override
-    public void setup() {
-        // Initialize slide motor
-        slideMotor = opMode.hardwareMap.get(DcMotorEx.class, "motor_slide");
-        slideMotor.setDirection(DcMotorEx.Direction.REVERSE);
-        slideMotor.setTargetPosition(MIN_HEIGHT);
-        slideMotor.setPower(MAX_POWER);
-        slideMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
-        slideMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
-        slideMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
-
-        // Initialize gripper servos
-        leftGripper = opMode.hardwareMap.get(Servo.class, "grip1");
-        rightGripper = opMode.hardwareMap.get(Servo.class, "grip2");
-    }
-
-    public void updateTelemetry() {
-        // Add some debug information
-        opMode.telemetry.addData("Target arm motor steps:", armMotorSteps);
-        opMode.telemetry.addData("Actual arm motor steps:", slideMotor.getCurrentPosition());
-        opMode.telemetry.addData("Arm Power:", slideMotor.getPower());
-        opMode.telemetry.addData("Arm Current (A):", slideMotor.getCurrent(CurrentUnit.AMPS));
-        opMode.telemetry.addData("Arm ZeroPower:", slideMotor.getZeroPowerBehavior());
-    }
-
-    public void readGamepad() {
-        // Set arm steps to predefined height with buttons
-        if (opMode.gamepad2.a) {
-            armMotorSteps = MIN_HEIGHT;
-        } else if (opMode.gamepad2.b) {
-            armMotorSteps = LOW_HEIGHT;
-        } else if (opMode.gamepad2.x) {
-            armMotorSteps = MEDIUM_HEIGHT;
-        } else if (opMode.gamepad2.y) {
-            armMotorSteps = MAX_HEIGHT;
-        }
-
-        // Increase arm steps by DPAD increments
-        if (opMode.gamepad2.dpad_up) {
-            armMotorSteps += INCREMENT_STEPS;
-            dPadPressed = true;
-        } else if (opMode.gamepad2.dpad_down) {
-            armMotorSteps -= INCREMENT_STEPS;
-            dPadPressed = true;
-        } else if (dPadPressed) {
-            armMotorSteps = slideMotor.getCurrentPosition();
-            dPadPressed = false;
-        }
-
-        setSlide(armMotorSteps);
-
-        // If motor is busy, run motor at max power.
-        // If motor is not busy, hold at current position or stop at lowest height
-        if (slideMotor.isBusy()) {
-            slideMotor.setPower(MAX_POWER);
-        } else {
-            slideMotor.setPower(HOLD_POWER);
-            if (slideMotor.getCurrentPosition() == MIN_HEIGHT) {
-                slideMotor.setPower(MIN_POWER);
-            }
-        }
-
-        if (opMode.gamepad1.right_trigger > 0.5) {
-            gripperClosed = true;
-        } else if (opMode.gamepad1.left_trigger > 0.5) {
-            gripperClosed = false;
-        }
-
-        if (gripperClosed) {
-            grab();
-        } else {
-            release();
-        }
-    }
-
-    public void grab() {
-        leftGripper.setPosition(GRIP_MAX + 0.1);
-        rightGripper.setPosition(GRIP_MIN - 0.1);
-    }
-
-    public void grabAndWait() {
-        grab();
-        while (leftGripper.getPosition() < GRIP_MAX || rightGripper.getPosition() > GRIP_MIN) {
-            opMode.idle();
-        }
-        opMode.sleep(300);
-    }
-
-    public void release() {
-        leftGripper.setPosition(GRIP_MIN);
-        rightGripper.setPosition(GRIP_MAX);
-    }
-
-    public void releaseAndWait() {
-        release();
-        while (leftGripper.getPosition() > GRIP_MIN || rightGripper.getPosition() < GRIP_MAX) {
-            opMode.idle();
-        }
-    }
-
-    public void setSlide(int steps) {
-        armMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
-        slideMotor.setTargetPosition(armMotorSteps);
-    }
-
-    public void setSlideAndWait(int steps) {
-        setSlide(steps);
-        opMode.blockOn(slideMotor);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
deleted file mode 100644
index 18a6261..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
+++ /dev/null
@@ -1,510 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.subsystem;
-
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.IMU;
-import com.qualcomm.robotcore.util.ElapsedTime;
-import com.qualcomm.robotcore.util.Range;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.teamcode.old.RobotOpMode;
-
-public class StrafeDrive extends Subsystem {
-    public IMU imu;
-    public DcMotor frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor;
-    private int flPos = 0;
-    private int frPos = 0;
-    private int blPos = 0;
-    private int brPos = 0;
-    private double botHeading;
-    private double limiter = 0.75;
-    private boolean field = false;
-
-    //
-    static final double HEADING_THRESHOLD = 1.0;
-    static final double P_TURN_GAIN = 0.02;     // Larger is more responsive, but also less stable
-    static final double P_DRIVE_GAIN = 0.03;
-    private double robotHeading = 0;
-    private double headingOffset = 0;
-    private double headingError = 0;
-
-    // These variable are declared here (as class members) so they can be updated in various methods,
-    // but still be displayed by sendTelemetry()
-    private double targetHeading = 0;
-    private double driveSpeed = 0;
-    private double turnSpeed = 0;
-    private double leftSpeed = 0;
-    private double rightSpeed = 0;
-
-
-    public StrafeDrive(RobotOpMode opMode) {
-        super(opMode);
-    }
-
-    public static int driveTicks(double inches) {
-        return (int) Math.round(inches * 54);
-    }
-
-    public static int strafeTicks(double inches) {
-        return (int) Math.round(inches * 56.25);
-    }
-
-    @Override
-    public void setup() {
-        frontLeftMotor = opMode.hardwareMap.get(DcMotor.class, "motor_front_left");
-        frontRightMotor = opMode.hardwareMap.get(DcMotor.class, "motor_front_right");
-        backLeftMotor = opMode.hardwareMap.get(DcMotor.class, "motor_back_left");
-        backRightMotor = opMode.hardwareMap.get(DcMotor.class, "motor_back_right");
-        imu = opMode.hardwareMap.get(IMU.class, "imu");
-        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
-        imu.initialize(parameters);
-        botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
-        backLeftMotor.setDirection(DcMotor.Direction.REVERSE);
-        frontLeftMotor.setDirection(DcMotor.Direction.REVERSE);
-    }
-
-    public void drive(int leftMove, int rightMove, float speed) {
-
-        flPos += leftMove;
-        frPos += rightMove;
-        blPos += leftMove;
-        brPos += rightMove;
-
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-
-        setModes(DcMotor.RunMode.RUN_TO_POSITION);
-
-        setPowers(speed);
-        opMode.blockOn(backLeftMotor, backRightMotor, frontLeftMotor, frontRightMotor);
-    }
-
-    public void driveInches(double leftInches, double rightInches, float speed) {
-
-        flPos += driveTicks(leftInches);
-        frPos += driveTicks(rightInches);
-        blPos += driveTicks(leftInches);
-        brPos += driveTicks(rightInches);
-
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-
-
-        setModes(DcMotor.RunMode.RUN_TO_POSITION);
-
-        setPowers(speed);
-//        while (opMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy()) {
-//            opMode.idle();
-//        }
-        opMode.blockOn(backLeftMotor, backRightMotor, frontLeftMotor, frontRightMotor);
-    }
-
-
-    public void strafe(int move, float speed) {
-
-        flPos += move;
-        frPos += move;
-        blPos -= move;
-        brPos -= move;
-
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-
-        setModes(DcMotor.RunMode.RUN_TO_POSITION);
-
-        setPowers(speed);
-        opMode.blockOn(backLeftMotor, backRightMotor, frontLeftMotor, frontRightMotor);
-    }
-
-    public void strafeInches(double inches, float speed) {
-
-        flPos += strafeTicks(inches);
-        frPos += strafeTicks(inches);
-        blPos -= strafeTicks(inches);
-        brPos -= strafeTicks(inches);
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-
-        setModes(DcMotor.RunMode.RUN_TO_POSITION);
-
-        setPowers(speed);
-        opMode.blockOn(backLeftMotor, backRightMotor, frontLeftMotor, frontRightMotor);
-    }
-
-
-    public void turnLeft(double rad, float speed) {
-        backLeftMotor.setPower(-speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(-speed);
-        frontRightMotor.setPower(speed);
-        setModes(DcMotor.RunMode.RUN_USING_ENCODER);
-        while (botHeading < rad) {
-
-            botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-            opMode.telemetry.addData("Heading: ", botHeading);
-
-            opMode.telemetry.update();
-            opMode.idle();
-
-        }
-        setPowers(0);
-        flPos = frontLeftMotor.getCurrentPosition();
-        frPos = frontRightMotor.getCurrentPosition();
-        blPos = backLeftMotor.getCurrentPosition();
-        brPos = backRightMotor.getCurrentPosition();
-
-    }
-
-    public void turnRight(double rad, float speed) {
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(-speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(-speed);
-        setModes(DcMotor.RunMode.RUN_USING_ENCODER);
-        while (botHeading > rad) {
-
-            botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-            opMode.telemetry.addData("Heading: ", botHeading);
-
-            opMode.telemetry.update();
-            opMode.idle();
-
-        }
-        setPowers(0);
-        flPos = frontLeftMotor.getCurrentPosition();
-        frPos = frontRightMotor.getCurrentPosition();
-        blPos = backLeftMotor.getCurrentPosition();
-        brPos = backRightMotor.getCurrentPosition();
-
-    }
-
-    public void updateByTwoGamepads() {
-
-        double forward;
-        double strafe;
-        double rotate;
-        double fieldForward;
-        double fieldStrafe;
-
-        if (opMode.gamepad1.a) {
-            limiter = 0.75;
-        } else if (opMode.gamepad1.b) {
-            limiter = 0.25;
-        }
-        if (opMode.gamepad1.x) {
-
-            imu.resetYaw();
-            field = true;
-        } else if (opMode.gamepad1.y) {
-            field = false;
-        }
-
-
-        forward = -opMode.gamepad1.left_stick_y * limiter;
-        strafe = opMode.gamepad1.left_stick_x * limiter;
-        rotate = opMode.gamepad1.right_stick_x * limiter;
-
-        if (Math.abs(-opMode.gamepad1.left_stick_y) < 0.01) {
-            forward = -opMode.gamepad2.left_stick_y * 0.4;
-            if (Math.abs(opMode.gamepad2.left_stick_y) < 0.02) {
-                forward = 0;
-            }
-        }
-        if (Math.abs(opMode.gamepad1.left_stick_x) < 0.01) {
-            strafe = 0;
-            strafe = opMode.gamepad2.left_stick_x * 0.4;
-            if (Math.abs(opMode.gamepad2.left_stick_x) < 0.02) {
-                strafe = 0;
-            }
-        }
-        if (Math.abs(opMode.gamepad1.right_stick_x) < 0.01) {
-            rotate = opMode.gamepad2.right_stick_x * 0.4;
-            if (Math.abs(opMode.gamepad2.right_stick_x) < 0.02) {
-                rotate = 0;
-            }
-        }
-        fieldForward = strafe * Math.sin(-botHeading) + forward * Math.cos(-botHeading);
-        fieldStrafe = strafe * Math.cos(-botHeading) - forward * Math.sin(-botHeading);
-
-        botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-        if (!field) {
-            frontLeftMotor.setPower((forward + strafe + rotate));
-            backLeftMotor.setPower((forward - strafe + rotate));
-            frontRightMotor.setPower((forward + strafe - rotate));
-            backRightMotor.setPower((forward - strafe - rotate));
-        } else {
-            frontLeftMotor.setPower((fieldForward + fieldStrafe + rotate));
-            backLeftMotor.setPower((fieldForward - fieldStrafe + rotate));
-            frontRightMotor.setPower((fieldForward + fieldStrafe - rotate));
-            backRightMotor.setPower((fieldForward - fieldStrafe - rotate));
-        }
-        opMode.telemetry.addData("Limiter: ", limiter);
-        opMode.telemetry.addData("Heading: ", botHeading);
-        opMode.telemetry.addData("Field Centric?: ", field);
-
-    }
-
-    public void updateByGamepad() {
-        double forward;
-        double strafe;
-        double rotate;
-
-        forward = -opMode.gamepad1.left_stick_y * limiter;
-        strafe = opMode.gamepad1.left_stick_x * limiter;
-        rotate = opMode.gamepad1.right_stick_x * limiter;
-        if (Math.abs(-opMode.gamepad1.left_stick_y) < 0.01) {
-            forward = 0;
-        }
-        if (Math.abs(opMode.gamepad1.left_stick_x) < 0.01) {
-            strafe = 0;
-        }
-        if (Math.abs(opMode.gamepad1.right_stick_x) < 0.01) {
-            rotate = 0;
-        }
-
-        frontLeftMotor.setPower((forward + strafe + rotate));
-        backLeftMotor.setPower((forward - strafe + rotate));
-        frontRightMotor.setPower((forward + strafe - rotate));
-        backRightMotor.setPower((forward - strafe - rotate));
-        opMode.telemetry.addData("Limiter: ", limiter);
-    }
-
-    public void setModes(DcMotor.RunMode mode) {
-        frontLeftMotor.setMode(mode);
-        frontRightMotor.setMode(mode);
-        backLeftMotor.setMode(mode);
-        backRightMotor.setMode(mode);
-    }
-
-    public void setPowers(float speed) {
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-    }
-
-    public void driveStraight(double maxDriveSpeed,
-                              double distance,
-                              double heading) {
-
-        // Ensure that the opmode is still active
-
-
-        // Determine new target position, and pass to motor controller
-        setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        flPos = (driveTicks(distance));
-        frPos = (driveTicks(distance));
-        brPos = (driveTicks(distance));
-        blPos = (driveTicks(distance));
-
-
-        // Set Target FIRST, then turn on RUN_TO_POSITION
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-
-        setModes(DcMotor.RunMode.RUN_TO_POSITION);
-
-        // Set the required driving speed  (must be positive for RUN_TO_POSITION)
-        // Start driving straight, and then enter the control loop
-        maxDriveSpeed = Math.abs(maxDriveSpeed);
-        moveRobot(maxDriveSpeed, 0);
-
-        // keep looping while we are still active, and BOTH motors are running.
-        while (opMode.isBusy(frontLeftMotor, backLeftMotor, frontRightMotor, frontLeftMotor)) {
-
-            // Determine required steering to keep on heading
-            turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN);
-
-            // if driving in reverse, the motor correction also needs to be reversed
-            if (distance < 0)
-                turnSpeed *= -1.0;
-
-            // Apply the turning correction to the current driving speed.
-            moveRobot(driveSpeed, turnSpeed);
-
-            // Display drive status for the driver.
-            sendTelemetry(true);
-        }
-
-        // Stop all motion & Turn off RUN_TO_POSITION
-        moveRobot(0, 0);
-        setModes(DcMotor.RunMode.RUN_USING_ENCODER);
-
-    }
-
-    /**
-     * Method to spin on central axis to point in a new direction.
-     * Move will stop if either of these conditions occur:
-     * 1) Move gets to the heading (angle)
-     * 2) Driver stops the opmode running.
-     *
-     * @param maxTurnSpeed Desired MAX speed of turn. (range 0 to +1.0)
-     * @param heading      Absolute Heading Angle (in Degrees) relative to last gyro reset.
-     *                     0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
-     *                     If a relative angle is required, add/subtract from current heading.
-     */
-    public void turnToHeading(double maxTurnSpeed, double heading) {
-
-        // Run getSteeringCorrection() once to pre-calculate the current error
-        getSteeringCorrection(heading, P_DRIVE_GAIN);
-
-        // keep looping while we are still active, and not on heading.
-        while ((Math.abs(headingError) > HEADING_THRESHOLD)) {
-
-            // Determine required steering to keep on heading
-            turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
-
-            // Clip the speed to the maximum permitted value.
-            turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
-
-            // Pivot in place by applying the turning correction
-            moveRobot(0, turnSpeed);
-
-            // Display drive status for the driver.
-            sendTelemetry(false);
-        }
-
-        // Stop all motion;
-        moveRobot(0, 0);
-    }
-
-    /**
-     * Method to obtain & hold a heading for a finite amount of time
-     * Move will stop once the requested time has elapsed
-     * This function is useful for giving the robot a moment to stabilize it's heading between movements.
-     *
-     * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0)
-     * @param heading      Absolute Heading Angle (in Degrees) relative to last gyro reset.
-     *                     0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
-     *                     If a relative angle is required, add/subtract from current heading.
-     * @param holdTime     Length of time (in seconds) to hold the specified heading.
-     */
-    public void holdHeading(double maxTurnSpeed, double heading, double holdTime) {
-
-        ElapsedTime holdTimer = new ElapsedTime();
-        holdTimer.reset();
-
-        // keep looping while we have time remaining.
-        while (holdTimer.time() < holdTime) {
-            // Determine required steering to keep on heading
-            turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
-
-            // Clip the speed to the maximum permitted value.
-            turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
-
-            // Pivot in place by applying the turning correction
-            moveRobot(0, turnSpeed);
-
-            // Display drive status for the driver.
-            sendTelemetry(false);
-        }
-
-        // Stop all motion;
-        moveRobot(0, 0);
-    }
-
-    // **********  LOW Level driving functions.  ********************
-
-    /**
-     * This method uses a Proportional Controller to determine how much steering correction is required.
-     *
-     * @param desiredHeading   The desired absolute heading (relative to last heading reset)
-     * @param proportionalGain Gain factor applied to heading error to obtain turning power.
-     * @return Turning power needed to get to required heading.
-     */
-    public double getSteeringCorrection(double desiredHeading, double proportionalGain) {
-        targetHeading = desiredHeading;  // Save for telemetry
-
-        // Get the robot heading by applying an offset to the IMU heading
-        robotHeading = getRawHeading() - headingOffset;
-
-        // Determine the heading current error
-        headingError = targetHeading - robotHeading;
-
-        // Normalize the error to be within +/- 180 degrees
-        while (headingError > 180) headingError -= 360;
-        while (headingError <= -180) headingError += 360;
-
-        // Multiply the error by the gain to determine the required steering correction/  Limit the result to +/- 1.0
-        return Range.clip(headingError * proportionalGain, -1, 1);
-    }
-
-    /**
-     * This method takes separate drive (fwd/rev) and turn (right/left) requests,
-     * combines them, and applies the appropriate speed commands to the left and right wheel motors.
-     *
-     * @param drive forward motor speed
-     * @param turn  clockwise turning motor speed.
-     */
-    public void moveRobot(double drive, double turn) {
-        driveSpeed = drive;     // save this value as a class member so it can be used by telemetry.
-        turnSpeed = turn;      // save this value as a class member so it can be used by telemetry.
-
-        leftSpeed = drive - turn;
-        rightSpeed = drive + turn;
-
-        // Scale speeds down if either one exceeds +/- 1.0;
-        double max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
-        if (max > 1.0) {
-            leftSpeed /= max;
-            rightSpeed /= max;
-        }
-
-        frontLeftMotor.setPower(leftSpeed);
-        frontRightMotor.setPower(rightSpeed);
-        backLeftMotor.setPower(leftSpeed);
-        backRightMotor.setPower(rightSpeed);
-    }
-
-    /**
-     * Display the various control parameters while driving
-     *
-     * @param straight Set to true if we are driving straight, and the encoder positions should be included in the telemetry.
-     */
-    private void sendTelemetry(boolean straight) {
-
-        if (straight) {
-            opMode.telemetry.addData("Motion", "Drive Straight");
-            opMode.telemetry.addData("Target Pos L:R", "%7d:%7d", flPos, frPos);
-            opMode.telemetry.addData("Actual Pos L:R", "%7d:%7d", frontLeftMotor.getCurrentPosition(),
-                    frontRightMotor.getCurrentPosition());
-        } else {
-            opMode.telemetry.addData("Motion", "Turning");
-        }
-
-        opMode.telemetry.addData("Angle Target:Current", "%5.2f:%5.0f", targetHeading, robotHeading);
-        opMode.telemetry.addData("Error:Steer", "%5.1f:%5.1f", headingError, turnSpeed);
-        opMode.telemetry.addData("Wheel Speeds L:R.", "%5.2f : %5.2f", leftSpeed, rightSpeed);
-        opMode.telemetry.update();
-    }
-
-    /**
-     * read the raw (un-offset Gyro heading) directly from the IMU
-     */
-    public double getRawHeading() {
-
-        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
-    }
-
-    /**
-     * Reset the "offset" heading back to zero
-     */
-    public void resetHeading() {
-        // Save a new heading offset equal to the current raw heading.
-        headingOffset = getRawHeading();
-        robotHeading = 0;
-    }
-
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
deleted file mode 100644
index 20ae19f..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
+++ /dev/null
@@ -1,13 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.subsystem;
-
-import org.firstinspires.ftc.teamcode.old.RobotOpMode;
-
-abstract public class Subsystem {
-    protected final RobotOpMode opMode;
-
-    public Subsystem(RobotOpMode opMode) {
-        this.opMode = opMode;
-    }
-
-    abstract public void setup();
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
deleted file mode 100644
index 81096e9..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
+++ /dev/null
@@ -1,40 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.subsystem;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.RobotOpMode;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class Webcam extends Subsystem {
-    public OpenCvCamera camera;
-    public final OpenCvPipeline pipeline;
-    public final String name;
-    public final static int WIDTH = 320;
-    public final static int HEIGHT = 240;
-
-    public Webcam(RobotOpMode opMode, String name, OpenCvPipeline pipeline) {
-        super(opMode);
-        this.name = name;
-        this.pipeline = pipeline;
-    }
-
-    @Override
-    public void setup() {
-        int monitorViewId = opMode.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", opMode.hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(opMode.hardwareMap.get(WebcamName.class, name), monitorViewId);
-        camera.setPipeline(pipeline);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
-            @Override
-            public void onOpened() {
-                camera.startStreaming(WIDTH, HEIGHT, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {
-            }
-        });
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
deleted file mode 100644
index 0af5e3e..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
+++ /dev/null
@@ -1,226 +0,0 @@
-/* Copyright (c) 2017-2020 FIRST. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted (subject to the limitations in the disclaimer below) provided that
- * the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of FIRST nor the names of its contributors may be used to endorse or
- * promote products derived from this software without specific prior written permission.
- *
- * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
- * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-package org.firstinspires.ftc.teamcode.old.teleop;
-
-import android.app.Activity;
-import android.graphics.Color;
-import android.view.View;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DistanceSensor;
-import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
-import com.qualcomm.robotcore.hardware.NormalizedRGBA;
-import com.qualcomm.robotcore.hardware.SwitchableLight;
-
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-
-/**
- * This is an example LinearOpMode that shows how to use a color sensor in a generic
- * way, regardless of which particular make or model of color sensor is used. The Op Mode
- * assumes that the color sensor is configured with a name of "sensor_color".
- * <p>
- * There will be some variation in the values measured depending on the specific sensor you are using.
- * <p>
- * You can increase the gain (a multiplier to make the sensor report higher values) by holding down
- * the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad.
- * <p>
- * If the color sensor has a light which is controllable from software, you can use the X button on
- * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have
- * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2.
- * <p>
- * If the color sensor also supports short-range distance measurements (usually via an infrared
- * proximity sensor), the reported distance will be written to telemetry. As of September 2020,
- * the only color sensors that support this are the ones from REV Robotics. These infrared proximity
- * sensor measurements are only useful at very small distances, and are sensitive to ambient light
- * and surface reflectivity. You should use a different sensor if you need precise distance measurements.
- * <p>
- * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
- * Remove or comment out the @Disabled line to add this Op Mode to the Driver Station OpMode list
- */
-@TeleOp(name = "Sensor: Color", group = "Sensor")
-public class SensorColor extends LinearOpMode {
-
-    /**
-     * The colorSensor field will contain a reference to our color sensor hardware object
-     */
-    NormalizedColorSensor colorSensor;
-
-    /**
-     * The relativeLayout field is used to aid in providing interesting visual feedback
-     * in this sample application; you probably *don't* need this when you use a color sensor on your
-     * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller.
-     */
-    View relativeLayout;
-
-    /**
-     * The runOpMode() method is the root of this Op Mode, as it is in all LinearOpModes.
-     * Our implementation here, though is a bit unusual: we've decided to put all the actual work
-     * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is
-     * that in this sample we're changing the background color of the robot controller screen as the
-     * Op Mode runs, and we want to be able to *guarantee* that we restore it to something reasonable
-     * and palatable when the Op Mode ends. The simplest way to do that is to use a try...finally
-     * block around the main, core logic, and an easy way to make that all clear was to separate
-     * the former from the latter in separate methods.
-     */
-    @Override
-    public void runOpMode() {
-
-        // Get a reference to the RelativeLayout so we can later change the background
-        // color of the Robot Controller app to match the hue detected by the RGB sensor.
-        int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
-        relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);
-
-        try {
-            runSample(); // actually execute the sample
-        } finally {
-            // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off
-            // as pure white, but it's too much work to dig out what actually was used, and this is good
-            // enough to at least make the screen reasonable again.
-            // Set the panel back to the default color
-            relativeLayout.post(new Runnable() {
-                public void run() {
-                    relativeLayout.setBackgroundColor(Color.WHITE);
-                }
-            });
-        }
-    }
-
-    protected void runSample() {
-        // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the
-        // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3)
-        // can give very low values (depending on the lighting conditions), which only use a small part
-        // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions,
-        // you should use a smaller gain than in dark conditions. If your gain is too high, all of the
-        // colors will report at or near 1, and you won't be able to determine what color you are
-        // actually looking at. For this reason, it's better to err on the side of a lower gain
-        // (but always greater than  or equal to 1).
-        float gain = 2;
-
-        // Once per loop, we will update this hsvValues array. The first element (0) will contain the
-        // hue, the second element (1) will contain the saturation, and the third element (2) will
-        // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
-        // for an explanation of HSV color.
-        final float[] hsvValues = new float[3];
-
-        // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current
-        // state of the X button on the gamepad
-        boolean xButtonPreviouslyPressed = false;
-        boolean xButtonCurrentlyPressed = false;
-
-        // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over
-        // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while
-        // the values you get from ColorSensor are dependent on the specific sensor you're using.
-        colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
-
-        // If possible, turn the light on in the beginning (it might already be on anyway,
-        // we just make sure it is if we can).
-        if (colorSensor instanceof SwitchableLight) {
-            ((SwitchableLight) colorSensor).enableLight(true);
-        }
-
-        // Wait for the start button to be pressed.
-        waitForStart();
-
-        // Loop until we are asked to stop
-        while (opModeIsActive()) {
-            // Explain basic gain information via telemetry
-            telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n");
-            telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n");
-
-            // Update the gain value if either of the A or B gamepad buttons is being held
-            if (gamepad1.a) {
-                // Only increase the gain by a small amount, since this loop will occur multiple times per second.
-                gain += 0.005;
-            } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful.
-                gain -= 0.005;
-            }
-
-            // Show the gain value via telemetry
-            telemetry.addData("Gain", gain);
-
-            // Tell the sensor our desired gain value (normally you would do this during initialization,
-            // not during the loop)
-            colorSensor.setGain(gain);
-
-            // Check the status of the X button on the gamepad
-            xButtonCurrentlyPressed = gamepad1.x;
-
-            // If the button state is different than what it was, then act
-            if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) {
-                // If the button is (now) down, then toggle the light
-                if (xButtonCurrentlyPressed) {
-                    if (colorSensor instanceof SwitchableLight) {
-                        SwitchableLight light = (SwitchableLight) colorSensor;
-                        light.enableLight(!light.isLightOn());
-                    }
-                }
-            }
-            xButtonPreviouslyPressed = xButtonCurrentlyPressed;
-
-            // Get the normalized colors from the sensor
-            NormalizedRGBA colors = colorSensor.getNormalizedColors();
-
-            /* Use telemetry to display feedback on the driver station. We show the red, green, and blue
-             * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent
-             * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
-             * for an explanation of HSV color. */
-
-            // Update the hsvValues array by passing it to Color.colorToHSV()
-            Color.colorToHSV(colors.toColor(), hsvValues);
-
-            telemetry.addLine()
-                    .addData("Red", "%.3f", colors.red)
-                    .addData("Green", "%.3f", colors.green)
-                    .addData("Blue", "%.3f", colors.blue);
-            telemetry.addLine()
-                    .addData("Hue", "%.3f", hsvValues[0])
-                    .addData("Saturation", "%.3f", hsvValues[1])
-                    .addData("Value", "%.3f", hsvValues[2]);
-            telemetry.addData("Alpha", "%.3f", colors.alpha);
-
-            /* If this color sensor also has a distance sensor, display the measured distance.
-             * Note that the reported distance is only useful at very close range, and is impacted by
-             * ambient light and surface reflectivity. */
-            if (colorSensor instanceof DistanceSensor) {
-                telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM));
-            }
-
-            telemetry.update();
-
-            // Change the Robot Controller's background color to match the color detected by the color sensor.
-            relativeLayout.post(new Runnable() {
-                public void run() {
-                    relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues));
-                }
-            });
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
deleted file mode 100644
index 9273b5b..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/LinearSlide.java
+++ /dev/null
@@ -1,117 +0,0 @@
-package org.firstinspires.ftc.teamcode.slide;
-
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.GRIP_MAX;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.GRIP_MIN;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MAX_HEIGHT;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MAX_POWER;
-import static org.firstinspires.ftc.teamcode.slide.SlideConstants.MIN_HEIGHT;
-
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.util.Range;
-
-public class LinearSlide {
-    public final DcMotorEx slideMotor;
-    public final Servo leftGripper, rightGripper;
-    private final HardwareMap hardwareMap;
-
-    private boolean gripperClosed = false;
-    private boolean dPadPressed = false;
-    private int targetArmMotorSteps = 0;
-
-    public LinearSlide(HardwareMap hardwareMap) {
-        this.hardwareMap = hardwareMap;
-
-        // Initialize slide motor
-        slideMotor = hardwareMap.get(DcMotorEx.class, "motor_slide");
-        slideMotor.setDirection(DcMotorEx.Direction.REVERSE);
-        slideMotor.setTargetPosition(MIN_HEIGHT);
-        slideMotor.setPower(MAX_POWER);
-        slideMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
-        slideMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
-        slideMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
-
-        // Initialize gripper servos
-        leftGripper = hardwareMap.get(Servo.class, "grip1");
-        rightGripper = hardwareMap.get(Servo.class, "grip2");
-    }
-
-    public void update() {
-        // Update PID
-    }
-
-//    public void updateTelemetry() {
-//        // Add some debug information
-//        opMode.telemetry.addData("Target arm motor steps:", targetArmMotorSteps);
-//        opMode.telemetry.addData("Actual arm motor steps:", slideMotor.getCurrentPosition());
-//        opMode.telemetry.addData("Arm Power:", slideMotor.getPower());
-//        opMode.telemetry.addData("Arm Current (A):", slideMotor.getCurrent(CurrentUnit.AMPS));
-//        opMode.telemetry.addData("Arm ZeroPower:", slideMotor.getZeroPowerBehavior());
-//    }
-
-//    public void readGamepad() {
-//        // Set arm steps to predefined height with buttons
-//        if (opMode.gamepad2.a) {
-//            targetArmMotorSteps = MIN_HEIGHT;
-//        } else if (opMode.gamepad2.b) {
-//            targetArmMotorSteps = LOW_HEIGHT;
-//        } else if (opMode.gamepad2.x) {
-//            targetArmMotorSteps = MEDIUM_HEIGHT;
-//        } else if (opMode.gamepad2.y) {
-//            targetArmMotorSteps = MAX_HEIGHT;
-//        }
-//
-//        // Increase arm steps by DPAD increments
-//        if (opMode.gamepad2.dpad_up) {
-//            targetArmMotorSteps += INCREMENT_STEPS;
-//            dPadPressed = true;
-//        } else if (opMode.gamepad2.dpad_down) {
-//            targetArmMotorSteps -= INCREMENT_STEPS;
-//            dPadPressed = true;
-//        } else if (dPadPressed) {
-//            targetArmMotorSteps = slideMotor.getCurrentPosition();
-//            dPadPressed = false;
-//        }
-//
-//        setSlide(targetArmMotorSteps);
-//
-//        // If motor is busy, run motor at max power.
-//        // If motor is not busy, hold at current position or stop at lowest height
-//        if (slideMotor.isBusy()) {
-//            slideMotor.setPower(MAX_POWER);
-//        } else {
-//            slideMotor.setPower(HOLD_POWER);
-//            if (slideMotor.getCurrentPosition() == MIN_HEIGHT) {
-//                slideMotor.setPower(MIN_POWER);
-//            }
-//        }
-//
-//        if (opMode.gamepad1.right_trigger > 0.5) {
-//            gripperClosed = true;
-//        } else if (opMode.gamepad1.left_trigger > 0.5) {
-//            gripperClosed = false;
-//        }
-//
-//        if (gripperClosed) {
-//            grab();
-//        } else {
-//            release();
-//        }
-//    }
-
-    public void grab() {
-        leftGripper.setPosition(GRIP_MAX + 0.1);
-        rightGripper.setPosition(GRIP_MIN - 0.1);
-    }
-
-    public void release() {
-        leftGripper.setPosition(GRIP_MIN);
-        rightGripper.setPosition(GRIP_MAX);
-    }
-
-    public void setSlide(int steps) {
-        targetArmMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
-        slideMotor.setTargetPosition(targetArmMotorSteps);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
deleted file mode 100644
index dfdbdae..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/slide/SlideConstants.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package org.firstinspires.ftc.teamcode.slide;
-
-import com.acmerobotics.dashboard.config.Config;
-
-@Config
-public class SlideConstants {
-    // Minimum power of the arm motor
-    public static final double MIN_POWER = 0;
-
-    // Holding power of the arm motor (when it is holding a position)
-    public static final double HOLD_POWER = 0.1;
-
-    // Maximum power of the arm motor
-    public static final double MAX_POWER = 1;
-
-    // Height (in encoder ticks) that the arm motor should go to reach the floor
-    public static final int MIN_HEIGHT = 0;
-
-    // Height (in encoder ticks) that the arm motor should go to reach the low junction
-    public static final int LOW_HEIGHT = 1600;
-
-    // Height (in encoder ticks) that the arm motor should go to reach the medium junction
-    public static final int MEDIUM_HEIGHT = 2760;
-
-    // Height (in encoder ticks) that the arm motor should go to reach the high junction
-    public static final int MAX_HEIGHT = 3870;
-
-    // Number of encoder ticks the arm should go up when the player presses the DPAD
-    public static final int INCREMENT_STEPS = 12;
-
-    // Minimum servo grip position
-    public static final double GRIP_MIN = 0;
-
-    // Maximum servo grip position
-    public static final double GRIP_MAX = 0.6;
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/DriveConstants.java
similarity index 99%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/DriveConstants.java
index 1d6b44b..fbde9de 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/DriveConstants.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.drive;
+package org.firstinspires.ftc.teamcode.subsystem.drive;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
similarity index 91%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
index e9e9b1d..d1dae17 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
@@ -1,16 +1,16 @@
-package org.firstinspires.ftc.teamcode.drive;
-
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+package org.firstinspires.ftc.teamcode.subsystem.drive;
+
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kV;
 
 import androidx.annotation.NonNull;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleTankDrive.java
similarity index 91%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleTankDrive.java
index cbfef3b..1e04278 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleTankDrive.java
@@ -1,4 +1,16 @@
-package org.firstinspires.ftc.teamcode.drive;
+package org.firstinspires.ftc.teamcode.subsystem.drive;
+
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kV;
 
 import androidx.annotation.NonNull;
 
@@ -36,18 +48,6 @@
 import java.util.Arrays;
 import java.util.List;
 
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
-
 /*
  * Simple tank drive hardware implementation for REV hardware.
  */
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/StandardTrackingWheelLocalizer.java
similarity index 98%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/StandardTrackingWheelLocalizer.java
index 7d039db..15c542c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/StandardTrackingWheelLocalizer.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.drive;
+package org.firstinspires.ftc.teamcode.subsystem.drive;
 
 import androidx.annotation.NonNull;
 
@@ -7,6 +7,7 @@
 import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+
 import org.firstinspires.ftc.teamcode.util.Encoder;
 
 import java.util.Arrays;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpTest.java
similarity index 96%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpTest.java
index fb5bfd4..59bae2d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpTest.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.teleop;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -7,7 +7,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 @TeleOp(name = "TeleOp Test")
 public class TeleOpTest extends LinearOpMode {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AutomaticFeedforwardTuner.java
similarity index 95%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AutomaticFeedforwardTuner.java
index d714b78..0611a37 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/AutomaticFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AutomaticFeedforwardTuner.java
@@ -1,8 +1,8 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_RPM;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.rpmToVelocity;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -15,7 +15,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.util.LoggingUtil;
 import org.firstinspires.ftc.teamcode.util.RegressionUtil;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/BackAndForth.java
similarity index 94%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/BackAndForth.java
index 807fe6b..3bdbcdd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/BackAndForth.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/BackAndForth.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
@@ -6,7 +6,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /*
  * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveVelocityPIDTuner.java
similarity index 92%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveVelocityPIDTuner.java
index 828eea3..056eb1a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/DriveVelocityPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveVelocityPIDTuner.java
@@ -1,10 +1,10 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kV;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -20,7 +20,7 @@
 import com.qualcomm.robotcore.util.RobotLog;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 import java.util.List;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java
similarity index 95%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java
index d563e94..3aeb85f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/FollowerPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java
@@ -1,11 +1,11 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
 
 /*
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
similarity index 93%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
index 73ac2eb..558c72d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/LocalizationTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
@@ -1,11 +1,11 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /**
  * This is a simple teleop routine for testing localization. Drive the robot around like a normal
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
similarity index 90%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
index 76db64f..1741751 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ManualFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
@@ -1,11 +1,11 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
-import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants.kV;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -21,7 +21,7 @@
 import com.qualcomm.robotcore.util.RobotLog;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 import java.util.Objects;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java
similarity index 95%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java
index 79c9df2..97613c7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxAngularVeloTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -10,7 +10,7 @@
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 import java.util.Objects;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java
similarity index 94%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java
index ccc4118..6730613 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MaxVelocityTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -11,8 +11,8 @@
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 import java.util.Objects;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java
similarity index 96%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java
index e638f91..69147e4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/MotorDirectionDebugger.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -7,7 +7,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /**
  * This is a simple teleop routine for debugging your motor configuration.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
similarity index 91%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
index 138771e..eb0dbfd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/SplineTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
@@ -7,7 +7,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /*
  * This is an example of a more complex path to really test the tuning.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java
similarity index 92%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java
index 90ff4d7..9e0c4ac 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StrafeTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /*
  * This is a simple routine to test translational drive capabilities.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java
similarity index 92%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java
index bee34af..f06f27c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/StraightTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /*
  * This is a simple routine to test translational drive capabilities.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java
similarity index 94%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java
index b852d3b..aadcf35 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackWidthTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -11,8 +11,8 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
-import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /*
  * This routine determines the effective track width. The procedure works by executing a point turn
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java
similarity index 95%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java
index 8be29de..4e68ee3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelForwardOffsetTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -12,8 +12,8 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.internal.system.Misc;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.StandardTrackingWheelLocalizer;
 
 /**
  * This routine determines the effective forward offset for the lateral tracking wheel.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java
similarity index 96%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java
index 22c8224..979134e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TrackingWheelLateralDistanceTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
@@ -7,8 +7,8 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.util.RobotLog;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.StandardTrackingWheelLocalizer;
 
 /**
  * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
similarity index 83%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
index 562b415..54ac297 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TurnTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
@@ -1,10 +1,10 @@
-package org.firstinspires.ftc.teamcode.opmode;
+package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 
 /*
  * This is a simple routine to test turning capabilities.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
index 66e5e9d..951e7d1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
@@ -15,10 +15,10 @@
 
 import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
 import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
-import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.drive.SampleTankDrive;
-import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleTankDrive;
+import org.firstinspires.ftc.teamcode.subsystem.drive.StandardTrackingWheelLocalizer;
 
 import java.io.File;
 import java.io.FileInputStream;

From bdec15ea9809d92d193e4b4864cd82302e23f064 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Mon, 20 Feb 2023 22:54:17 -0500
Subject: [PATCH 16/43] added simple teleop

---
 TeamCode/build.gradle                         |   1 +
 .../ftc/teamcode/auto/AutoTest.java           |  31 ---
 .../ftc/teamcode/pipeline/BoeDetection.java   | 185 ++++++++++++++++++
 .../ftc/teamcode/pipeline/PoleDetection.java  | 103 ++++++++++
 .../teamcode/pipeline/SleeveDetectionNew.java |  89 +++++++++
 .../pipeline/SleeveDetectionRight.java        | 144 ++++++++++++++
 .../subsystem/colorsensor/ColorSensor.java    |  68 +++++++
 .../subsystem/drive/SampleMecanumDrive.java   |   3 +-
 .../teamcode/subsystem/slide/LinearSlide.java |  57 ++++++
 .../ftc/teamcode/teleop/OnePlayerTeleOp.java  | 109 +++++++++++
 .../ftc/teamcode/teleop/TeleOpTest.java       |  90 ---------
 11 files changed, 757 insertions(+), 123 deletions(-)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/BoeDetection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/PoleDetection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionNew.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionRight.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/colorsensor/ColorSensor.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpTest.java

diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 45085d2..d656b32 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -33,4 +33,5 @@ dependencies {
 
 dependencies {
     implementation 'org.openftc:easyopencv:1.5.2'
+    implementation 'com.google.ar:core:1.30.0'
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
deleted file mode 100644
index d1b00bd..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoTest.java
+++ /dev/null
@@ -1,31 +0,0 @@
-package org.firstinspires.ftc.teamcode.auto;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
-
-@Autonomous(name = "Auto Test")
-@Disabled
-public class AutoTest extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        drive.followTrajectory(drive.trajectoryBuilder(new Pose2d())
-                .splineTo(new Vector2d(30, 30), 0)
-                .build());
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/BoeDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/BoeDetection.java
new file mode 100644
index 0000000..d225651
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/BoeDetection.java
@@ -0,0 +1,185 @@
+package org.firstinspires.ftc.teamcode.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class BoeDetection extends OpenCvPipeline {
+    /*
+    YELLOW  = Parking Left
+    CYAN    = Parking Middle
+    MAGENTA = Parking Right
+     */
+
+    public enum Cone {
+        RED,
+        BLUE,
+        UNDECIDED
+    }
+
+    public enum Distance {
+        CLOSE,
+        MID,
+        FAR,
+        UNSEEN
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static final int X = 55;
+    private static final int Y = 120;
+    private static final int STEP = 10;
+    private static final int WIDTH = 25;
+    private static final int HEIGHT = 15;
+    private static final Point FAR_TOPLEFT_ANCHOR_POINT = new Point(X, Y);
+    private static final Point CLOSE_TOPLEFT_ANCHOR_POINT = new Point(X - (2 * STEP), Y - (2 * STEP));
+
+    // Width and height for the bounding box
+    public static int FAR_REGION_WIDTH = WIDTH;
+    public static int FAR_REGION_HEIGHT = HEIGHT;
+    public static int MID_REGION_WIDTH = WIDTH + STEP + STEP;
+    public static int MID_REGION_HEIGHT = HEIGHT + STEP + STEP;
+    public static int CLOSE_REGION_WIDTH = WIDTH + STEP + STEP + STEP + STEP;
+    public static int CLOSE_REGION_HEIGHT = HEIGHT + STEP + STEP + STEP + STEP;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_red_bounds = new Scalar(100, 0, 0, 255),
+            upper_red_bounds = new Scalar(255, 150, 150, 255),
+            lower_blue_bounds = new Scalar(0, 0, 100, 255),
+            upper_blue_bounds = new Scalar(150, 150, 255, 255);
+
+    // Color definitions
+    private final Scalar
+            RED = new Scalar(255, 0, 0),
+            BLUE = new Scalar(0, 0, 255),
+            BLACK = new Scalar(0, 0, 0);
+
+    // Percent and mat definitions
+    private double redPercentC, bluePercentC, redPercentM, bluePercentM, redPercentF, bluePercentF;
+    private final Mat redMatC = new Mat();
+    private final Mat blueMatC = new Mat();
+    private Mat redMatF = new Mat();
+    private Mat blueMatF = new Mat();
+    private Mat redMatM = new Mat();
+    private Mat blueMatM = new Mat();
+    private Mat blurredMatClose = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point((2 * STEP), (2 * STEP));
+    Point sleeve_pointB = new Point(
+            (2 * STEP) + FAR_REGION_WIDTH,
+            (2 * STEP) + FAR_REGION_HEIGHT);
+
+    Point sleeve_pointC = new Point(STEP, STEP);
+    Point sleeve_pointD = new Point(
+            STEP + MID_REGION_WIDTH,
+            STEP + MID_REGION_HEIGHT);
+
+    Point sleeve_pointE = new Point(
+            CLOSE_TOPLEFT_ANCHOR_POINT.x,
+            CLOSE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointF = new Point(
+            CLOSE_TOPLEFT_ANCHOR_POINT.x + CLOSE_REGION_WIDTH,
+            CLOSE_TOPLEFT_ANCHOR_POINT.y + CLOSE_REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile Cone cone = Cone.UNDECIDED;
+    private volatile Distance distance = Distance.UNSEEN;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMatClose, new Size(5, 5));
+        blurredMatClose = blurredMatClose.submat(new Rect(sleeve_pointE, sleeve_pointF));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMatClose, blurredMatClose, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMatClose, lower_red_bounds, upper_red_bounds, redMatC);
+        Core.inRange(blurredMatClose, lower_blue_bounds, upper_blue_bounds, blueMatC);
+
+        redMatM = redMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
+        blueMatM = blueMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
+
+        redMatF = redMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
+        blueMatF = blueMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+
+        // Gets color specific values
+        redPercentC = Core.countNonZero(redMatC);
+        bluePercentC = Core.countNonZero(blueMatC);
+
+        redPercentM = Core.countNonZero(redMatM);
+        bluePercentM = Core.countNonZero(blueMatM);
+
+        redPercentF = Core.countNonZero(redMatF);
+        bluePercentF = Core.countNonZero(blueMatF);
+
+
+        // Calculates the highest amount of pixels being covered on each side
+        double maxPercent = Math.max(redPercentF, bluePercentF);
+
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (maxPercent == redPercentF) {
+            if (redPercentC >= 150) {
+                cone = Cone.RED;
+                distance = Distance.CLOSE;
+            } else if (redPercentM >= 100) {
+                cone = Cone.RED;
+                distance = Distance.MID;
+            } else if (redPercentF >= 50) {
+                cone = Cone.RED;
+                distance = Distance.FAR;
+            } else {
+                cone = Cone.UNDECIDED;
+                distance = Distance.UNSEEN;
+            }
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    RED,
+                    2
+            );
+        } else if (maxPercent == bluePercentC && bluePercentC >= 50) {
+            cone = Cone.BLUE;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    BLUE,
+                    2
+            );
+        } else {
+            cone = Cone.UNDECIDED;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    BLACK,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        redMatC.release();
+        blueMatC.release();
+        kernel.release();
+
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public Cone getPosition() {
+        return cone;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/PoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/PoleDetection.java
new file mode 100644
index 0000000..0ceddc2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/PoleDetection.java
@@ -0,0 +1,103 @@
+package org.firstinspires.ftc.teamcode.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class PoleDetection extends OpenCvPipeline {
+    private volatile boolean isPole = false;
+
+    // TOPLEFT anchor point for the bounding box
+    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(50, 85);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 50;
+    public static int REGION_HEIGHT = 30;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
+            upper_yellow_bounds = new Scalar(255, 255, 150, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255);
+
+    // Percent and mat definitions
+    private double yelPercent;
+    private final Mat yelMat = new Mat();
+    private Mat blurredMat = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+//
+//        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+//        Core.inRange(input, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+
+        // Gets color specific values
+//        Imgproc.Canny(yelMat, yelMat, 100, 200);
+        yelPercent = Core.countNonZero(yelMat);
+
+        // Calculates the highest amount of pixels being covered on each side
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (yelPercent >= 100) {
+            isPole = true;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        } else {
+            isPole = false;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        blurredMat.release();
+        yelMat.release();
+        kernel.release();
+
+        return input;
+    }
+
+    public boolean getPole() {
+        return isPole;
+    }
+
+    public double getPercent() {
+        return yelPercent;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionNew.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionNew.java
new file mode 100644
index 0000000..ca4a385
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionNew.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class SleeveDetectionNew extends OpenCvPipeline {
+
+    public enum ParkingPosition {
+        LEFT,
+        CENTER,
+        RIGHT
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(65, 50);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 25;
+    public static int REGION_HEIGHT = 15;
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255),
+            MAGENTA = new Scalar(255, 0, 255);
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile ParkingPosition position = ParkingPosition.LEFT;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Get the submat frame, and then sum all the values
+        Mat areaMat = input.submat(new Rect(sleeve_pointA, sleeve_pointB));
+        Scalar sumColors = Core.sumElems(areaMat);
+
+        double minColor = Math.min(sumColors.val[0], Math.min(sumColors.val[1], sumColors.val[2]));
+
+        if (sumColors.val[0] == minColor) {
+            position = ParkingPosition.CENTER;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        } else if (sumColors.val[1] == minColor) {
+            position = ParkingPosition.RIGHT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    MAGENTA,
+                    2
+            );
+        } else {
+            position = ParkingPosition.LEFT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        }
+
+        // Release and return input
+        areaMat.release();
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public ParkingPosition getPosition() {
+        return position;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionRight.java
new file mode 100644
index 0000000..fdf28fb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pipeline/SleeveDetectionRight.java
@@ -0,0 +1,144 @@
+package org.firstinspires.ftc.teamcode.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class SleeveDetectionRight extends OpenCvPipeline {
+    /*
+    YELLOW  = Parking Left
+    CYAN    = Parking Middle
+    MAGENTA = Parking Right
+     */
+
+    public enum ParkingPosition {
+        LEFT,
+        CENTER,
+        RIGHT
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(125, 175);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 25;
+    public static int REGION_HEIGHT = 15;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+//            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
+//            upper_yellow_bounds = new Scalar(255, 255, 150, 255),
+//            lower_cyan_bounds = new Scalar(0, 120, 120, 255),
+//            upper_cyan_bounds = new Scalar(150, 255, 255, 255),
+//            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
+//            upper_magenta_bounds = new Scalar(255, 170, 255, 255);
+
+            lower_yellow_bounds = new Scalar(120, 120, 0, 255),
+            upper_yellow_bounds = new Scalar(255, 255, 70, 255),
+            lower_cyan_bounds = new Scalar(2, 82, 176, 255),
+            upper_cyan_bounds = new Scalar(48, 255, 255, 255),
+            lower_magenta_bounds = new Scalar(125, 3, 176, 255),
+            upper_magenta_bounds = new Scalar(255, 43, 255, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255),
+            MAGENTA = new Scalar(255, 0, 255);
+
+    // Percent and mat definitions
+    private double yelPercent, cyaPercent, magPercent;
+    private final Mat yelMat = new Mat();
+    private final Mat cyaMat = new Mat();
+    private final Mat magMat = new Mat();
+    private Mat blurredMat = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile ParkingPosition position = ParkingPosition.LEFT;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+        Imgproc.cvtColor(input, blurredMat, Imgproc.COLOR_RGB2HSV);
+        blurredMat.convertTo(blurredMat, -1, 1, 10);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+        Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
+        Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
+
+        // Gets color specific values
+        yelPercent = Core.countNonZero(yelMat);
+        cyaPercent = Core.countNonZero(cyaMat);
+        magPercent = Core.countNonZero(magMat);
+
+        // Calculates the highest amount of pixels being covered on each side
+        double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
+
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (maxPercent == yelPercent) {
+            position = ParkingPosition.LEFT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        } else if (maxPercent == cyaPercent) {
+            position = ParkingPosition.CENTER;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        } else if (maxPercent == magPercent) {
+            position = ParkingPosition.RIGHT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    MAGENTA,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        blurredMat.release();
+        yelMat.release();
+        cyaMat.release();
+        magMat.release();
+        kernel.release();
+
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public ParkingPosition getPosition() {
+        return position;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/colorsensor/ColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/colorsensor/ColorSensor.java
new file mode 100644
index 0000000..e13bef0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/colorsensor/ColorSensor.java
@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode.subsystem.colorsensor;
+
+import android.graphics.Color;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+
+@Config
+public class ColorSensor {
+    private final NormalizedColorSensor colorSensor;
+    private NormalizedRGBA colors;
+    private float[] hsvValues = new float[3];
+
+    public float gain = 2;
+
+    public ColorSensor(HardwareMap hardwareMap) {
+        colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
+        colors = colorSensor.getNormalizedColors();
+        colorSensor.setGain(gain);
+    }
+
+    public void update() {
+        colors = colorSensor.getNormalizedColors();
+        Color.colorToHSV(colors.toColor(), hsvValues);
+    }
+
+    public float getHue() {
+        update();
+        return hsvValues[0];
+    }
+
+    public float getSaturation() {
+        update();
+        return hsvValues[1];
+    }
+
+    public float getValue() {
+        update();
+        return hsvValues[2];
+    }
+
+    public float getRed() {
+        update();
+        return colors.red;
+    }
+
+    public float getBlue() {
+        update();
+        return colors.blue;
+    }
+
+    public float getGreen() {
+        update();
+        return colors.green;
+    }
+
+    public float getAlpha() {
+        update();
+        return colors.alpha;
+    }
+
+    public float getGain() {
+        update();
+        return colorSensor.getGain();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
index d1dae17..21d3912 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
@@ -93,10 +93,9 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
         imu = hardwareMap.get(IMU.class, "imu");
         IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
                 RevHubOrientationOnRobot.LogoFacingDirection.UP,
-                RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
         imu.initialize(parameters);
 
-
         leftFront = hardwareMap.get(DcMotorEx.class, "motor_front_left");
         leftRear = hardwareMap.get(DcMotorEx.class, "motor_back_left");
         rightRear = hardwareMap.get(DcMotorEx.class, "motor_front_right"); // ???
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
new file mode 100644
index 0000000..5d0c373
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.subsystem.slide;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+
+@Config
+public class LinearSlide {
+    public static double MIN_POWER = 0;
+    public static double HOLD_POWER = 0.1;
+    public static double MAX_POWER = 1;
+
+    public static int MIN_HEIGHT = 0;
+    public static int LOW_HEIGHT = 1600;
+    public static int MEDIUM_HEIGHT = 2760;
+    public static int MAX_HEIGHT = 3870;
+    public static int INCREMENT_STEPS = 20;
+
+    public static double GRIP_MIN = 0;
+    public static double GRIP_MAX = 0.6;
+
+    public DcMotorEx slideMotor;
+    public Servo leftGripper, rightGripper;
+
+    private boolean gripperClosed = false;
+    private boolean dPadPressed = false;
+    private int armMotorSteps = 0;
+
+    public LinearSlide(HardwareMap hardwareMap) {
+        slideMotor = hardwareMap.get(DcMotorEx.class, "motor_slide");
+        slideMotor.setDirection(DcMotorEx.Direction.REVERSE);
+        slideMotor.setTargetPosition(MIN_HEIGHT);
+        slideMotor.setPower(MAX_POWER);
+        slideMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
+        slideMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
+        slideMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
+
+        leftGripper = hardwareMap.get(Servo.class, "grip1");
+        rightGripper = hardwareMap.get(Servo.class, "grip2");
+    }
+
+    public void setSlide(int steps) {
+        armMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
+        slideMotor.setTargetPosition(armMotorSteps);
+    }
+
+    public void ungrab() {
+        leftGripper.setPosition(GRIP_MIN);
+        rightGripper.setPosition(GRIP_MAX);
+    }
+
+    public void grab() {
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
new file mode 100644
index 0000000..e5e809f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
@@ -0,0 +1,109 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
+
+@Config
+@TeleOp
+public class OnePlayerTeleOp extends LinearOpMode {
+    public static double HIGH_POWER = 0.85;
+    public static double LOW_POWER = 0.35;
+    public static double DEAD_ZONE_P1 = 0.05;
+    public static double DEAD_ZONE_P2 = 0.05;
+
+    SampleMecanumDrive drive;
+    LinearSlide slide;
+
+    double limiter = HIGH_POWER;
+    boolean fieldCentric = false;
+    double botHeading;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        drive = new SampleMecanumDrive(hardwareMap);
+        slide = new LinearSlide(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            updateDriveByGamepad();
+            drive.update();
+
+            telemetry.addData("Limiter: ", limiter);
+            telemetry.addData("Heading: ", botHeading);
+            telemetry.addData("Field Centric?: ", fieldCentric);
+            telemetry.update();
+        }
+    }
+
+    public void updateDriveByGamepad() {
+        botHeading = drive.getRawExternalHeading();
+
+        if (gamepad1.a) {
+            limiter = HIGH_POWER;
+        } else if (gamepad1.b) {
+            limiter = LOW_POWER;
+        }
+
+        if (gamepad1.x) {
+            drive.setPoseEstimate(new Pose2d());
+            fieldCentric = true;
+        } else if (gamepad1.y) {
+            fieldCentric = false;
+        }
+
+        double forward = -gamepad1.left_stick_y * limiter;
+        double strafe = gamepad1.left_stick_x * limiter;
+        double rotate = gamepad1.right_stick_x * limiter;
+
+        if (Math.abs(gamepad1.left_stick_y) < DEAD_ZONE_P1) {
+            forward = -gamepad2.left_stick_y * LOW_POWER;
+            if (Math.abs(gamepad2.left_stick_y) < DEAD_ZONE_P2) {
+                forward = 0;
+            }
+        }
+
+        if (Math.abs(gamepad1.left_stick_x) < DEAD_ZONE_P1) {
+            strafe = gamepad2.left_stick_x * LOW_POWER;
+            if (Math.abs(gamepad2.left_stick_x) < DEAD_ZONE_P2) {
+                strafe = 0;
+            }
+        }
+
+        if (Math.abs(gamepad1.right_stick_x) < DEAD_ZONE_P1) {
+            rotate = gamepad2.right_stick_x * LOW_POWER;
+            if (Math.abs(gamepad2.right_stick_x) < DEAD_ZONE_P2) {
+                rotate = 0;
+            }
+        }
+
+        double fieldForward = strafe * Math.sin(-botHeading) + forward * Math.cos(-botHeading);
+        double fieldStrafe = strafe * Math.cos(-botHeading) - forward * Math.sin(-botHeading);
+
+        if (!fieldCentric) {
+            drive.setWeightedDrivePower(
+                    new Pose2d(
+                            forward,
+                            -strafe,
+                            -rotate
+                    )
+            );
+        } else {
+            drive.setWeightedDrivePower(
+                    new Pose2d(
+                            fieldForward,
+                            -fieldStrafe,
+                            -rotate
+                    )
+            );
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpTest.java
deleted file mode 100644
index 59bae2d..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpTest.java
+++ /dev/null
@@ -1,90 +0,0 @@
-package org.firstinspires.ftc.teamcode.teleop;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
-
-@TeleOp(name = "TeleOp Test")
-public class TeleOpTest extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-
-        waitForStart();
-
-        if (isStopRequested()) return;
-
-        double limiter = 0.75;
-        boolean field = true;
-
-        while (opModeIsActive()) {
-            double heading = drive.getExternalHeading();
-
-            if (gamepad1.a) {
-                limiter = 0.75;
-            } else if (gamepad1.b) {
-                limiter = 0.25;
-            }
-            if (gamepad1.x) {
-                drive.setPoseEstimate(new Pose2d());
-                field = true;
-            } else if (gamepad1.y) {
-                field = false;
-            }
-
-            double forward = -gamepad1.left_stick_y * limiter;
-            double strafe = gamepad1.left_stick_x * limiter;
-            double rotate = gamepad1.right_stick_x * limiter;
-
-            if (Math.abs(-gamepad1.left_stick_y) < 0.01) {
-                forward = -gamepad2.left_stick_y * 0.4;
-                if (Math.abs(gamepad2.left_stick_y) < 0.02) {
-                    forward = 0;
-                }
-            }
-            if (Math.abs(gamepad1.left_stick_x) < 0.01) {
-                strafe = gamepad2.left_stick_x * 0.4;
-                if (Math.abs(gamepad2.left_stick_x) < 0.02) {
-                    strafe = 0;
-                }
-            }
-            if (Math.abs(gamepad1.right_stick_x) < 0.01) {
-                rotate = gamepad2.right_stick_x * 0.4;
-                if (Math.abs(gamepad2.right_stick_x) < 0.02) {
-                    rotate = 0;
-                }
-            }
-            double fieldForward = strafe * Math.sin(-heading) + forward * Math.cos(-heading);
-            double fieldStrafe = strafe * Math.cos(-heading) - forward * Math.sin(-heading);
-
-            if (field) {
-                drive.setMotorPowers(
-                        fieldForward + fieldStrafe + rotate,
-                        fieldForward - fieldStrafe + rotate,
-                        fieldForward - fieldStrafe - rotate,
-                        fieldForward + fieldStrafe - rotate
-                );
-            } else {
-                drive.setMotorPowers(
-                        forward + strafe + rotate,
-                        forward - strafe + rotate,
-                        forward - strafe - rotate,
-                        forward + strafe - rotate
-                );
-            }
-
-            telemetry.addData("Limiter: ", limiter);
-            telemetry.addData("Heading: ", heading);
-            telemetry.addData("Field Centric?: ", field);
-
-            drive.update();
-        }
-    }
-}

From f60ad9227ec13ac5edd92a59964369e62010855c Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Tue, 21 Feb 2023 00:00:22 -0500
Subject: [PATCH 17/43] added linear slide to simple teleop

---
 .../teamcode/subsystem/slide/LinearSlide.java | 16 +++++-
 .../ftc/teamcode/teleop/OnePlayerTeleOp.java  | 57 +++++++++++++++++++
 2 files changed, 72 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
index 5d0c373..8998c25 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
@@ -46,12 +46,26 @@ public void setSlide(int steps) {
         slideMotor.setTargetPosition(armMotorSteps);
     }
 
+    public void update() {
+        // If motor is busy, run motor at max power.
+        // If motor is not busy, hold at current position or stop at lowest height
+        if (slideMotor.isBusy()) {
+            slideMotor.setPower(MAX_POWER);
+        } else {
+            slideMotor.setPower(HOLD_POWER);
+            if (slideMotor.getCurrentPosition() == MIN_HEIGHT) {
+                slideMotor.setPower(MIN_POWER);
+            }
+        }
+    }
+
     public void ungrab() {
         leftGripper.setPosition(GRIP_MIN);
         rightGripper.setPosition(GRIP_MAX);
     }
 
     public void grab() {
-
+        leftGripper.setPosition(GRIP_MAX + 0.1);
+        rightGripper.setPosition(GRIP_MIN - 0.1);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
index e5e809f..8e323fd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
@@ -1,11 +1,21 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.INCREMENT_STEPS;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.LOW_HEIGHT;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.MAX_HEIGHT;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.MEDIUM_HEIGHT;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.MIN_HEIGHT;
+
+import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
 
@@ -24,8 +34,13 @@ public class OnePlayerTeleOp extends LinearOpMode {
     boolean fieldCentric = false;
     double botHeading;
 
+    int armMotorSteps = 0;
+    boolean dPadPressed = false;
+
     @Override
     public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
         drive = new SampleMecanumDrive(hardwareMap);
         slide = new LinearSlide(hardwareMap);
 
@@ -35,15 +50,57 @@ public void runOpMode() throws InterruptedException {
 
         while (opModeIsActive()) {
             updateDriveByGamepad();
+            updateSlideByGamepad();
+
             drive.update();
+            slide.update();
 
             telemetry.addData("Limiter: ", limiter);
             telemetry.addData("Heading: ", botHeading);
             telemetry.addData("Field Centric?: ", fieldCentric);
+
+            telemetry.addData("Target arm motor steps:", armMotorSteps);
+            telemetry.addData("Actual arm motor steps:", slide.slideMotor.getCurrentPosition());
+            telemetry.addData("Arm Power:", slide.slideMotor.getPower());
+            telemetry.addData("Arm Current (A):", slide.slideMotor.getCurrent(CurrentUnit.AMPS));
+            telemetry.addData("Arm ZeroPower:", slide.slideMotor.getZeroPowerBehavior());
+
             telemetry.update();
         }
     }
 
+    public void updateSlideByGamepad() {
+        // Set arm steps to predefined height with buttons
+        if (gamepad2.a) {
+            armMotorSteps = MIN_HEIGHT;
+        } else if (gamepad2.b) {
+            armMotorSteps = LOW_HEIGHT;
+        } else if (gamepad2.x) {
+            armMotorSteps = MEDIUM_HEIGHT;
+        } else if (gamepad2.y) {
+            armMotorSteps = MAX_HEIGHT;
+        }
+
+        // Increase arm steps by DPAD increments
+        if (gamepad2.dpad_up) {
+            armMotorSteps += INCREMENT_STEPS;
+            dPadPressed = true;
+        } else if (gamepad2.dpad_down) {
+            armMotorSteps -= INCREMENT_STEPS;
+            dPadPressed = true;
+        } else if (dPadPressed) {
+            dPadPressed = false;
+        }
+
+        slide.setSlide(armMotorSteps);
+
+        if (gamepad1.right_trigger > 0.5) {
+            slide.grab();
+        } else if (gamepad1.left_trigger > 0.5) {
+            slide.ungrab();
+        }
+    }
+
     public void updateDriveByGamepad() {
         botHeading = drive.getRawExternalHeading();
 

From c57e730b03345039374a44014520ffcafcb5dc92 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Tue, 21 Feb 2023 00:07:26 -0500
Subject: [PATCH 18/43] made auto skeleton

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 39 +++++++++++++++++++
 .../ftc/teamcode/subsystem/webcam/Webcam.java | 32 +++++++++++++++
 2 files changed, 71 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/webcam/Webcam.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
new file mode 100644
index 0000000..cf4ef51
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
+import org.firstinspires.ftc.teamcode.subsystem.webcam.Webcam;
+
+@Config
+@Autonomous
+public class AutoSkeleton extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        LinearSlide slide = new LinearSlide(hardwareMap);
+        SleeveDetectionNew sleeveDetection = new SleeveDetectionNew();
+        Webcam cam = new Webcam(hardwareMap, sleeveDetection);
+
+        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+                .forward(30)
+                .build();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.followTrajectory(trajectory);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/webcam/Webcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/webcam/Webcam.java
new file mode 100644
index 0000000..36204cf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/webcam/Webcam.java
@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.subsystem.webcam;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class Webcam {
+    public OpenCvCamera camera;
+    public final static int WIDTH = 320;
+    public final static int HEIGHT = 240;
+
+    public Webcam(HardwareMap hardwareMap, OpenCvPipeline pipeline) {
+        int monitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), monitorViewId);
+        camera.setPipeline(pipeline);
+
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            @Override
+            public void onOpened() {
+                camera.startStreaming(WIDTH, HEIGHT, OpenCvCameraRotation.SIDEWAYS_LEFT);
+            }
+
+            @Override
+            public void onError(int errorCode) {
+            }
+        });
+    }
+}

From a8240ab1ba6cb24dd7f3d932f40d67a8a1abcdd5 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Tue, 21 Feb 2023 00:10:03 -0500
Subject: [PATCH 19/43] filed all tuning opmodes into the tuning group

---
 .../java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java  | 2 ++
 .../ftc/teamcode/tuning/AutomaticFeedforwardTuner.java          | 2 +-
 .../org/firstinspires/ftc/teamcode/tuning/BackAndForth.java     | 2 +-
 .../ftc/teamcode/tuning/DriveVelocityPIDTuner.java              | 2 +-
 .../org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java | 2 +-
 .../org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java | 2 +-
 .../ftc/teamcode/tuning/ManualFeedforwardTuner.java             | 2 +-
 .../firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java  | 2 +-
 .../org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java | 2 +-
 .../ftc/teamcode/tuning/MotorDirectionDebugger.java             | 2 +-
 .../java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java  | 2 +-
 .../java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java  | 2 +-
 .../org/firstinspires/ftc/teamcode/tuning/StraightTest.java     | 2 +-
 .../org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java  | 2 +-
 .../ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java    | 2 +-
 .../ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java  | 2 +-
 .../java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java    | 2 +-
 17 files changed, 18 insertions(+), 16 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index cf4ef51..30d98f9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -10,6 +10,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
+import org.firstinspires.ftc.teamcode.subsystem.colorsensor.ColorSensor;
 import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
 import org.firstinspires.ftc.teamcode.subsystem.webcam.Webcam;
@@ -25,6 +26,7 @@ public void runOpMode() throws InterruptedException {
         LinearSlide slide = new LinearSlide(hardwareMap);
         SleeveDetectionNew sleeveDetection = new SleeveDetectionNew();
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
+        ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
         Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
                 .forward(30)
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AutomaticFeedforwardTuner.java
index 0611a37..ba4d8eb 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AutomaticFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AutomaticFeedforwardTuner.java
@@ -33,7 +33,7 @@
  *      regression.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class AutomaticFeedforwardTuner extends LinearOpMode {
     public static double MAX_POWER = 0.7;
     public static double DISTANCE = 100; // in
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/BackAndForth.java
index 3bdbcdd..20ddad0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/BackAndForth.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/BackAndForth.java
@@ -25,7 +25,7 @@
  * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class BackAndForth extends LinearOpMode {
 
     public static double DISTANCE = 50;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveVelocityPIDTuner.java
index 056eb1a..90336dc 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveVelocityPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveVelocityPIDTuner.java
@@ -49,7 +49,7 @@
  * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class DriveVelocityPIDTuner extends LinearOpMode {
     public static double DISTANCE = 72; // in
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java
index 3aeb85f..6cd445a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/FollowerPIDTuner.java
@@ -22,7 +22,7 @@
  * These coefficients can be tuned live in dashboard.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class FollowerPIDTuner extends LinearOpMode {
     public static double DISTANCE = 48; // in
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
index 558c72d..72fb722 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
@@ -14,7 +14,7 @@
  * exercise is to ascertain whether the localizer has been configured properly (note: the pure
  * encoder localizer heading may be significantly off if the track width has not been tuned).
  */
-@TeleOp(group = "drive")
+@TeleOp(group = "tuning")
 public class LocalizationTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
index 1741751..a71d12a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
@@ -41,7 +41,7 @@
  * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class ManualFeedforwardTuner extends LinearOpMode {
     public static double DISTANCE = 72; // in
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java
index 97613c7..0b5e5fb 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxAngularVeloTuner.java
@@ -23,7 +23,7 @@
  */
 
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class MaxAngularVeloTuner extends LinearOpMode {
     public static double RUNTIME = 4.0;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java
index 6730613..3b9d0a1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java
@@ -25,7 +25,7 @@
  * Further fine tuning of kF may be desired.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class MaxVelocityTuner extends LinearOpMode {
     public static double RUNTIME = 2.0;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java
index 69147e4..97cb52e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java
@@ -38,7 +38,7 @@
  * Uncomment the @Disabled tag below to use this opmode.
  */
 @Config
-@TeleOp(group = "drive")
+@TeleOp(group = "tuning")
 public class MotorDirectionDebugger extends LinearOpMode {
     public static double MOTOR_POWER = 0.7;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
index eb0dbfd..c3e5bb9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
@@ -13,7 +13,7 @@
  * This is an example of a more complex path to really test the tuning.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class SplineTest extends LinearOpMode {
     public static int TRIALS = 1;
     public static int DELAY = 2000;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java
index 9e0c4ac..f58e7ae 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java
@@ -15,7 +15,7 @@
  * This is a simple routine to test translational drive capabilities.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class StrafeTest extends LinearOpMode {
     public static double DISTANCE = 60; // in
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java
index f06f27c..37d5cb2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java
@@ -15,7 +15,7 @@
  * This is a simple routine to test translational drive capabilities.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class StraightTest extends LinearOpMode {
     public static double DISTANCE = 60; // in
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java
index aadcf35..d24502e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java
@@ -24,7 +24,7 @@
  * accurate track width estimate is important or else the angular constraints will be thrown off.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class TrackWidthTuner extends LinearOpMode {
     public static double ANGLE = 180; // deg
     public static int NUM_TRIALS = 5;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java
index 4e68ee3..2108295 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java
@@ -35,7 +35,7 @@
  * satisfactory result is produced.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
     public static double ANGLE = 180; // deg
     public static int NUM_TRIALS = 5;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java
index 979134e..f24396f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java
@@ -62,7 +62,7 @@
  * precision. The heading should still line up.
  */
 @Config
-@TeleOp(group = "drive")
+@TeleOp(group = "tuning")
 public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
     public static int NUM_TURNS = 10;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
index 54ac297..1b28823 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
@@ -10,7 +10,7 @@
  * This is a simple routine to test turning capabilities.
  */
 @Config
-@Autonomous(group = "drive")
+@Autonomous(group = "tuning")
 public class TurnTest extends LinearOpMode {
     public static double ANGLE = 90; // deg
 

From 02b5e88588a8e6ea5c964ecbe6d0660b0a9b8415 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 23 Feb 2023 20:49:55 -0500
Subject: [PATCH 20/43] roadrunner day 1

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 34 ++++++++++++++++---
 .../subsystem/drive/SampleMecanumDrive.java   |  2 +-
 .../teamcode/subsystem/slide/LinearSlide.java |  1 +
 ...PlayerTeleOp.java => TwoPlayerTeleOp.java} |  5 +--
 4 files changed, 35 insertions(+), 7 deletions(-)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/{OnePlayerTeleOp.java => TwoPlayerTeleOp.java} (98%)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 30d98f9..8383f09 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -4,7 +4,7 @@
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
@@ -14,10 +14,15 @@
 import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
 import org.firstinspires.ftc.teamcode.subsystem.webcam.Webcam;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
 
 @Config
 @Autonomous
 public class AutoSkeleton extends LinearOpMode {
+    public static double x = 55;
+    public static double y = -21;
+    public static double f = 2;
+
     @Override
     public void runOpMode() throws InterruptedException {
         Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -28,14 +33,35 @@ public void runOpMode() throws InterruptedException {
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
         ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
-        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
-                .forward(30)
+        TrajectorySequence traj1 = drive.trajectorySequenceBuilder(new Pose2d())
+                .addDisplacementMarker(slide::grab)
+                .waitSeconds(2)
+                .addDisplacementMarker(() -> slide.setSlide(60))
+                .waitSeconds(1.5)
+                .splineToLinearHeading(new Pose2d(10, -22, Math.toRadians(55)), Math.toRadians(0))
+                .build();
+
+        TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end())
+                .lineToConstantHeading(new Vector2d(x, y))
                 .build();
 
+        TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end())
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
+                .forward(4.5)
+                .waitSeconds(1.5)
+                .addDisplacementMarker(slide::ungrab)
+                .waitSeconds(1.5)
+                .back(4.5)
+                .addDisplacementMarker(() -> slide.setSlide(60))
+                .build();
+
+
         waitForStart();
 
         if (isStopRequested()) return;
 
-        drive.followTrajectory(trajectory);
+        drive.followTrajectorySequence(traj1);
+        drive.followTrajectorySequence(traj2);
+        drive.followTrajectorySequence(traj3);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
index 21d3912..3e76e8a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
@@ -73,7 +73,7 @@ public class SampleMecanumDrive extends MecanumDrive {
     private DcMotorEx leftFront, leftRear, rightRear, rightFront;
     private List<DcMotorEx> motors;
 
-    private IMU imu;
+    public IMU imu;
     private VoltageSensor batteryVoltageSensor;
 
     public SampleMecanumDrive(HardwareMap hardwareMap) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
index 8998c25..1a26ac8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
@@ -67,5 +67,6 @@ public void ungrab() {
     public void grab() {
         leftGripper.setPosition(GRIP_MAX + 0.1);
         rightGripper.setPosition(GRIP_MIN - 0.1);
+        
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
similarity index 98%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
index 8e323fd..1bb529d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OnePlayerTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
@@ -21,7 +21,7 @@
 
 @Config
 @TeleOp
-public class OnePlayerTeleOp extends LinearOpMode {
+public class TwoPlayerTeleOp extends LinearOpMode {
     public static double HIGH_POWER = 0.85;
     public static double LOW_POWER = 0.35;
     public static double DEAD_ZONE_P1 = 0.05;
@@ -45,7 +45,7 @@ public void runOpMode() throws InterruptedException {
         slide = new LinearSlide(hardwareMap);
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
+        drive.imu.resetYaw();
         waitForStart();
 
         while (opModeIsActive()) {
@@ -112,6 +112,7 @@ public void updateDriveByGamepad() {
 
         if (gamepad1.x) {
             drive.setPoseEstimate(new Pose2d());
+            drive.imu.resetYaw();
             fieldCentric = true;
         } else if (gamepad1.y) {
             fieldCentric = false;

From 4c9c6308d0e25390d54e9c465be336318119ef2a Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Fri, 24 Feb 2023 10:57:15 -0500
Subject: [PATCH 21/43] roadrunner day 1

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 25 ++++++++++++++++---
 1 file changed, 22 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 8383f09..a3804a1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -19,7 +19,7 @@
 @Config
 @Autonomous
 public class AutoSkeleton extends LinearOpMode {
-    public static double x = 55;
+    public static double x = 58;
     public static double y = -21;
     public static double f = 2;
 
@@ -33,11 +33,11 @@ public void runOpMode() throws InterruptedException {
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
         ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
+        int height = 500;
+
         TrajectorySequence traj1 = drive.trajectorySequenceBuilder(new Pose2d())
                 .addDisplacementMarker(slide::grab)
-                .waitSeconds(2)
                 .addDisplacementMarker(() -> slide.setSlide(60))
-                .waitSeconds(1.5)
                 .splineToLinearHeading(new Pose2d(10, -22, Math.toRadians(55)), Math.toRadians(0))
                 .build();
 
@@ -52,10 +52,28 @@ public void runOpMode() throws InterruptedException {
                 .addDisplacementMarker(slide::ungrab)
                 .waitSeconds(1.5)
                 .back(4.5)
+//                .addDisplacementMarker(() -> slide.setSlide(60))
+                .build();
+
+        TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end())
                 .addDisplacementMarker(() -> slide.setSlide(60))
+                .splineToLinearHeading(new Pose2d(50, -10, Math.toRadians(0)), Math.toRadians(0))
                 .build();
 
+        TrajectorySequence trajRepeat = drive.trajectorySequenceBuilder(new Pose2d())
+                .addDisplacementMarker(() -> slide.setSlide(height))
+                .splineToLinearHeading(new Pose2d(0, 40, Math.toRadians(90)), Math.toRadians(0))
+                .addDisplacementMarker(slide::grab)
+                .splineToLinearHeading(new Pose2d(0, 0, Math.toRadians(0)), Math.toRadians(0))
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
+                .forward(4.5)
+                .waitSeconds(1.5)
+                .addDisplacementMarker(slide::ungrab)
+                .waitSeconds(1.5)
+                .back(4.5)
+                .build();
 
+        slide.ungrab();
         waitForStart();
 
         if (isStopRequested()) return;
@@ -63,5 +81,6 @@ public void runOpMode() throws InterruptedException {
         drive.followTrajectorySequence(traj1);
         drive.followTrajectorySequence(traj2);
         drive.followTrajectorySequence(traj3);
+        drive.followTrajectorySequence(traj4);
     }
 }

From 18a0a47e02aacad73d785219dbc6255e873310f6 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Fri, 24 Feb 2023 11:31:48 -0500
Subject: [PATCH 22/43] roadrunner day 2

---
 .idea/deploymentTargetDropDown.xml             | 17 +++++++++++++++++
 .../ftc/teamcode/auto/AutoSkeleton.java        | 18 +++---------------
 2 files changed, 20 insertions(+), 15 deletions(-)
 create mode 100644 .idea/deploymentTargetDropDown.xml

diff --git a/.idea/deploymentTargetDropDown.xml b/.idea/deploymentTargetDropDown.xml
new file mode 100644
index 0000000..0fdcd9a
--- /dev/null
+++ b/.idea/deploymentTargetDropDown.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="deploymentTargetDropDown">
+    <targetSelectedWithDropDown>
+      <Target>
+        <type value="QUICK_BOOT_TARGET" />
+        <deviceKey>
+          <Key>
+            <type value="VIRTUAL_DEVICE_PATH" />
+            <value value="$PROJECT_DIR$/../.android/avd/Pixel_3a_API_33_x86_64.avd" />
+          </Key>
+        </deviceKey>
+      </Target>
+    </targetSelectedWithDropDown>
+    <timeTargetWasSelectedWithDropDown value="2023-02-05T05:57:21.620605Z" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index a3804a1..237f910 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -4,7 +4,6 @@
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
@@ -39,27 +38,18 @@ public void runOpMode() throws InterruptedException {
                 .addDisplacementMarker(slide::grab)
                 .addDisplacementMarker(() -> slide.setSlide(60))
                 .splineToLinearHeading(new Pose2d(10, -22, Math.toRadians(55)), Math.toRadians(0))
-                .build();
-
-        TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end())
-                .lineToConstantHeading(new Vector2d(x, y))
-                .build();
-
-        TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end())
+                .splineToLinearHeading(new Pose2d(58, 21, Math.toRadians(55)), Math.toRadians(0))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
                 .forward(4.5)
                 .waitSeconds(1.5)
                 .addDisplacementMarker(slide::ungrab)
                 .waitSeconds(1.5)
                 .back(4.5)
-//                .addDisplacementMarker(() -> slide.setSlide(60))
-                .build();
-
-        TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end())
                 .addDisplacementMarker(() -> slide.setSlide(60))
                 .splineToLinearHeading(new Pose2d(50, -10, Math.toRadians(0)), Math.toRadians(0))
                 .build();
 
+
         TrajectorySequence trajRepeat = drive.trajectorySequenceBuilder(new Pose2d())
                 .addDisplacementMarker(() -> slide.setSlide(height))
                 .splineToLinearHeading(new Pose2d(0, 40, Math.toRadians(90)), Math.toRadians(0))
@@ -79,8 +69,6 @@ public void runOpMode() throws InterruptedException {
         if (isStopRequested()) return;
 
         drive.followTrajectorySequence(traj1);
-        drive.followTrajectorySequence(traj2);
-        drive.followTrajectorySequence(traj3);
-        drive.followTrajectorySequence(traj4);
+
     }
 }

From db8c89f7d9a140b8877eac966ab216d81f12c63b Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 24 Feb 2023 12:31:01 -0500
Subject: [PATCH 23/43] added funny auto using path drawing

---
 .../ftc/teamcode/auto/AutoFunny.java          | 44 +++++++++++++++++++
 1 file changed, 44 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
new file mode 100644
index 0000000..c40d12b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
+import org.firstinspires.ftc.teamcode.subsystem.colorsensor.ColorSensor;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
+import org.firstinspires.ftc.teamcode.subsystem.webcam.Webcam;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+@Config
+@Autonomous
+public class AutoFunny extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        LinearSlide slide = new LinearSlide(hardwareMap);
+        SleeveDetectionNew sleeveDetection = new SleeveDetectionNew();
+        Webcam cam = new Webcam(hardwareMap, sleeveDetection);
+        ColorSensor colorSensor = new ColorSensor(hardwareMap);
+
+        TrajectorySequence traj = drive.trajectorySequenceBuilder(new Pose2d(-36.48, -70.71, Math.toRadians(90.00)))
+                .splineTo(new Vector2d(-28.13, -13.82), Math.toRadians(33.69))
+                .splineTo(new Vector2d(-19.93, -69.75), Math.toRadians(-80.09))
+                .build();
+        drive.setPoseEstimate(traj.start());
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.followTrajectorySequence(traj);
+    }
+}

From 72c9c85e3d6d24392b2d11fd211fe5571d9614ca Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 24 Feb 2023 14:38:52 -0500
Subject: [PATCH 24/43] added complex spline path

---
 .../ftc/teamcode/auto/AutoFunny.java             | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
index c40d12b..1bc98ee 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
@@ -29,10 +29,20 @@ public void runOpMode() throws InterruptedException {
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
         ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
-        TrajectorySequence traj = drive.trajectorySequenceBuilder(new Pose2d(-36.48, -70.71, Math.toRadians(90.00)))
-                .splineTo(new Vector2d(-28.13, -13.82), Math.toRadians(33.69))
-                .splineTo(new Vector2d(-19.93, -69.75), Math.toRadians(-80.09))
+//        TrajectorySequence traj = drive.trajectorySequenceBuilder(new Pose2d(-36.85, -66.01, Math.toRadians(88.15)))
+//                .splineTo(new Vector2d(-6.40, -15.31), Math.toRadians(2.23))
+//                .splineToConstantHeading(new Vector2d(26.97, -56.94), Math.toRadians(199.98))
+//                .splineToConstantHeading(new Vector2d(-37.01, -12.88), Math.toRadians(175.60))
+//                .build();
+        TrajectorySequence traj = drive.trajectorySequenceBuilder(new Pose2d(-37.34, -65.52, Math.toRadians(88.15)))
+                .splineTo(new Vector2d(-6.40, -15.31), Math.toRadians(2.23))
+                .splineToConstantHeading(new Vector2d(26.97, -56.94), Math.toRadians(199.98))
+                .splineToConstantHeading(new Vector2d(-37.01, -12.88), Math.toRadians(175.60))
+                .splineToConstantHeading(new Vector2d(-58.39, -18.71), Math.toRadians(-84.62))
+                .splineToConstantHeading(new Vector2d(23.73, -16.44), Math.toRadians(2.79))
+                .splineTo(new Vector2d(-37.34, -65.68), Math.toRadians(-90.00))
                 .build();
+
         drive.setPoseEstimate(traj.start());
 
         waitForStart();

From 5e2bd73548d6a9504c1360939bbc95330fe80246 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Mon, 27 Feb 2023 19:44:59 -0500
Subject: [PATCH 25/43] First cone complete (~90% acc)

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 36 ++++++++++++++++---
 1 file changed, 32 insertions(+), 4 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 237f910..8b7e1e4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -4,6 +4,7 @@
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
@@ -38,7 +39,7 @@ public void runOpMode() throws InterruptedException {
                 .addDisplacementMarker(slide::grab)
                 .addDisplacementMarker(() -> slide.setSlide(60))
                 .splineToLinearHeading(new Pose2d(10, -22, Math.toRadians(55)), Math.toRadians(0))
-                .splineToLinearHeading(new Pose2d(58, 21, Math.toRadians(55)), Math.toRadians(0))
+                .splineToLinearHeading(new Pose2d(58, -21, Math.toRadians(55)), Math.toRadians(0))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
                 .forward(4.5)
                 .waitSeconds(1.5)
@@ -56,19 +57,46 @@ public void runOpMode() throws InterruptedException {
                 .addDisplacementMarker(slide::grab)
                 .splineToLinearHeading(new Pose2d(0, 0, Math.toRadians(0)), Math.toRadians(0))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .forward(4.5)
+                .forward(5)
                 .waitSeconds(1.5)
                 .addDisplacementMarker(slide::ungrab)
                 .waitSeconds(1.5)
-                .back(4.5)
+                .back(5)
                 .build();
 
+        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(-31.08, -64.54, Math.toRadians(90.00)))
+                .addDisplacementMarker(slide::grab)
+                .splineToConstantHeading(new Vector2d(-12.33, -52.49), Math.toRadians(70.00))
+                .splineToConstantHeading(new Vector2d(-12.53, -31.84), Math.toRadians(90.00))
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
+                .splineToConstantHeading(new Vector2d(-24.38, -6.79), Math.toRadians(180.00))
+                .waitSeconds(0.4)
+                .addDisplacementMarker(slide::ungrab)
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+
+                .splineTo(new Vector2d(-24.38, -14.63), Math.toRadians(90.00))
+                .splineToConstantHeading(new Vector2d(-45.61, -12.72), Math.toRadians(180.00))
+                .lineToSplineHeading(new Pose2d(-63.01, -12.33, Math.toRadians(180.00)))
+
+                .addDisplacementMarker(slide::grab)
+                .build();
+        drive.setPoseEstimate(trajA.start());
+//        TrajectorySequence trajB = drive.trajectorySequenceBuilder(new Pose2d(-24.38, -6.79, Math.toRadians(90.00)))
+//                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+//                .splineToSplineHeading(new Pose2d(-24.00, -13.29, Math.toRadians(180.00)), Math.toRadians(90.00))
+//                .splineToConstantHeading(new Vector2d(-63.97, -12.14), Math.toRadians(180.00))
+//                .addDisplacementMarker(slide::grab)
+//                .build();
+
+
         slide.ungrab();
+        drive.imu.resetYaw();
         waitForStart();
 
         if (isStopRequested()) return;
 
-        drive.followTrajectorySequence(traj1);
+        drive.followTrajectorySequence(trajA);
+//        drive.followTrajectorySequence(trajB);
 
     }
 }

From 5ee3fd3ab3ce2bf977e7c856f2a04313d357ecb0 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Tue, 28 Feb 2023 12:43:33 -0500
Subject: [PATCH 26/43] working on second cone(stuck in stop error)

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 26 ++++++++++---------
 1 file changed, 14 insertions(+), 12 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 8b7e1e4..8089737 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -71,22 +71,24 @@ public void runOpMode() throws InterruptedException {
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
                 .splineToConstantHeading(new Vector2d(-24.38, -6.79), Math.toRadians(180.00))
                 .waitSeconds(0.4)
+                .build();
+        drive.setPoseEstimate(trajA.start());
+
+        TrajectorySequence trajB = drive.trajectorySequenceBuilder(new Pose2d(-23.94, -7.22, Math.toRadians(90.00)))
                 .addDisplacementMarker(slide::ungrab)
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
-
-                .splineTo(new Vector2d(-24.38, -14.63), Math.toRadians(90.00))
+                .splineToConstantHeading(new Vector2d(-23.81, -13.00), Math.toRadians(90.00))
                 .splineToConstantHeading(new Vector2d(-45.61, -12.72), Math.toRadians(180.00))
-                .lineToSplineHeading(new Pose2d(-63.01, -12.33, Math.toRadians(180.00)))
-
+                .addDisplacementMarker(() -> slide.setSlide(480))
+                //first stack 480
+                //second stack 360
+                //third stack 180
+                //fourth stack 0
+                .lineToSplineHeading(new Pose2d(-64.59, -12.16, Math.toRadians(180.00)))
                 .addDisplacementMarker(slide::grab)
+                .splineToConstantHeading(new Vector2d(-45.61, -12.72), Math.toRadians(180.00))
+
                 .build();
-        drive.setPoseEstimate(trajA.start());
-//        TrajectorySequence trajB = drive.trajectorySequenceBuilder(new Pose2d(-24.38, -6.79, Math.toRadians(90.00)))
-//                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
-//                .splineToSplineHeading(new Pose2d(-24.00, -13.29, Math.toRadians(180.00)), Math.toRadians(90.00))
-//                .splineToConstantHeading(new Vector2d(-63.97, -12.14), Math.toRadians(180.00))
-//                .addDisplacementMarker(slide::grab)
-//                .build();
 
 
         slide.ungrab();
@@ -96,7 +98,7 @@ public void runOpMode() throws InterruptedException {
         if (isStopRequested()) return;
 
         drive.followTrajectorySequence(trajA);
-//        drive.followTrajectorySequence(trajB);
+        drive.followTrajectorySequence(trajB);
 
     }
 }

From b5cd65f80edf49c01eaeab829ec5b5dc946d2e95 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 2 Mar 2023 14:46:14 -0500
Subject: [PATCH 27/43] Second cone working (all anthony)

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 57 +++++++++++++------
 .../teamcode/subsystem/slide/LinearSlide.java | 12 +++-
 2 files changed, 50 insertions(+), 19 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 8089737..9799210 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -33,7 +33,8 @@ public void runOpMode() throws InterruptedException {
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
         ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
-        int height = 500;
+        int[] heights = {480, 360, 180, 0, 0};
+        int time = 0;
 
         TrajectorySequence traj1 = drive.trajectorySequenceBuilder(new Pose2d())
                 .addDisplacementMarker(slide::grab)
@@ -52,7 +53,7 @@ public void runOpMode() throws InterruptedException {
 
 
         TrajectorySequence trajRepeat = drive.trajectorySequenceBuilder(new Pose2d())
-                .addDisplacementMarker(() -> slide.setSlide(height))
+                .addDisplacementMarker(() -> slide.setSlide(480))
                 .splineToLinearHeading(new Pose2d(0, 40, Math.toRadians(90)), Math.toRadians(0))
                 .addDisplacementMarker(slide::grab)
                 .splineToLinearHeading(new Pose2d(0, 0, Math.toRadians(0)), Math.toRadians(0))
@@ -69,28 +70,48 @@ public void runOpMode() throws InterruptedException {
                 .splineToConstantHeading(new Vector2d(-12.33, -52.49), Math.toRadians(70.00))
                 .splineToConstantHeading(new Vector2d(-12.53, -31.84), Math.toRadians(90.00))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .splineToConstantHeading(new Vector2d(-24.38, -6.79), Math.toRadians(180.00))
-                .waitSeconds(0.4)
+                .splineToConstantHeading(new Vector2d(-24.38, -7), Math.toRadians(180.00))
+                .waitSeconds(1)
                 .build();
         drive.setPoseEstimate(trajA.start());
-
-        TrajectorySequence trajB = drive.trajectorySequenceBuilder(new Pose2d(-23.94, -7.22, Math.toRadians(90.00)))
+//new Pose2d(-24.38, -7, Math.toRadians(90.00))
+        TrajectorySequence trajB = drive.trajectorySequenceBuilder(trajA.end())
                 .addDisplacementMarker(slide::ungrab)
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
-                .splineToConstantHeading(new Vector2d(-23.81, -13.00), Math.toRadians(90.00))
-                .splineToConstantHeading(new Vector2d(-45.61, -12.72), Math.toRadians(180.00))
+//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
+                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
                 .addDisplacementMarker(() -> slide.setSlide(480))
-                //first stack 480
-                //second stack 360
-                //third stack 180
-                //fourth stack 0
-                .lineToSplineHeading(new Pose2d(-64.59, -12.16, Math.toRadians(180.00)))
-                .addDisplacementMarker(slide::grab)
-                .splineToConstantHeading(new Vector2d(-45.61, -12.72), Math.toRadians(180.00))
-
+                .lineToSplineHeading(new Pose2d(-64.00, -7.78, Math.toRadians(180.00)))
+                .build();
+//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
+        TrajectorySequence trajC = drive.trajectorySequenceBuilder(trajB.end())
+                .addDisplacementMarker(() -> {
+                    slide.grab();
+                    sleep(500);
+                })
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(1200);
+                    sleep(1000);
+                })
+//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
+                .lineToConstantHeading(new Vector2d(-48.61, -10.72))
+                .lineToSplineHeading(new Pose2d(-27, -10.75, Math.toRadians(90.00)))
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(-27, -3))
+                .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(200);
+                })
                 .build();
 
-
+        //Cone 1: 480
+        //Cone 2: 360
+        //Cone 3: 180
+        //Cone 4: 0
+        //Cone 5: 0
         slide.ungrab();
         drive.imu.resetYaw();
         waitForStart();
@@ -99,6 +120,6 @@ public void runOpMode() throws InterruptedException {
 
         drive.followTrajectorySequence(trajA);
         drive.followTrajectorySequence(trajB);
-
+        drive.followTrajectorySequence(trajC);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
index 1a26ac8..5e30ed7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
@@ -67,6 +67,16 @@ public void ungrab() {
     public void grab() {
         leftGripper.setPosition(GRIP_MAX + 0.1);
         rightGripper.setPosition(GRIP_MIN - 0.1);
-        
+
+    }
+
+    public void grabAndRaise(int steps) {
+        leftGripper.setPosition(GRIP_MAX + 0.1);
+        rightGripper.setPosition(GRIP_MIN - 0.1);
+//        while (leftGripper.getPosition() < GRIP_MAX - 0.1 || rightGripper.getPosition() > GRIP_MIN + 0.1) {
+//            wait();
+//        }
+        armMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
+        slideMotor.setTargetPosition(armMotorSteps);
     }
 }

From f790a6cc8f2c388381ad19fd0f44256a6fb0eeaf Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Thu, 2 Mar 2023 16:12:32 -0500
Subject: [PATCH 28/43] worked on linear actuator and third cone

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 36 ++++++++++++++++++-
 .../teamcode/subsystem/slide/LinearSlide.java | 15 ++++++--
 .../ftc/teamcode/teleop/TwoPlayerTeleOp.java  | 14 ++++++++
 3 files changed, 62 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 9799210..be1b93d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -95,7 +95,7 @@ public void runOpMode() throws InterruptedException {
                 })
 //                .addDisplacementMarker(() -> slide.grabAndRaise(800))
                 .lineToConstantHeading(new Vector2d(-48.61, -10.72))
-                .lineToSplineHeading(new Pose2d(-27, -10.75, Math.toRadians(90.00)))
+                .lineToSplineHeading(new Pose2d(-24, -10.75, Math.toRadians(90.00)))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(LinearSlide.MAX_HEIGHT);
                     sleep(1000);
@@ -106,6 +106,38 @@ public void runOpMode() throws InterruptedException {
                     sleep(200);
                 })
                 .build();
+        TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
+                .addDisplacementMarker(slide::ungrab)
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
+                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+                .addDisplacementMarker(() -> slide.setSlide(360))
+                .lineToSplineHeading(new Pose2d(-64.00, -7.78, Math.toRadians(180.00)))
+                .build();
+//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
+        TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajD.end())
+                .addDisplacementMarker(() -> {
+                    slide.grab();
+                    sleep(500);
+                })
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(1200);
+                    sleep(1000);
+                })
+//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
+                .lineToConstantHeading(new Vector2d(-48.61, -10.72))
+                .lineToSplineHeading(new Pose2d(-24, -10.75, Math.toRadians(90.00)))
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(-27, -3))
+                .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(200);
+                })
+                .build();
+
 
         //Cone 1: 480
         //Cone 2: 360
@@ -121,5 +153,7 @@ public void runOpMode() throws InterruptedException {
         drive.followTrajectorySequence(trajA);
         drive.followTrajectorySequence(trajB);
         drive.followTrajectorySequence(trajC);
+        drive.followTrajectorySequence(trajD);
+        drive.followTrajectorySequence(trajE);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
index 5e30ed7..b260106 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
@@ -21,11 +21,13 @@ public class LinearSlide {
     public static double GRIP_MIN = 0;
     public static double GRIP_MAX = 0.6;
 
+    public static double ACTUATOR_MIN = 0.17;
+    public static double ACTUATOR_MAX = 0.82;
+
     public DcMotorEx slideMotor;
     public Servo leftGripper, rightGripper;
+    public Servo linearActuator;
 
-    private boolean gripperClosed = false;
-    private boolean dPadPressed = false;
     private int armMotorSteps = 0;
 
     public LinearSlide(HardwareMap hardwareMap) {
@@ -39,6 +41,8 @@ public LinearSlide(HardwareMap hardwareMap) {
 
         leftGripper = hardwareMap.get(Servo.class, "grip1");
         rightGripper = hardwareMap.get(Servo.class, "grip2");
+
+        linearActuator = hardwareMap.get(Servo.class, "linear_actuator");
     }
 
     public void setSlide(int steps) {
@@ -67,7 +71,14 @@ public void ungrab() {
     public void grab() {
         leftGripper.setPosition(GRIP_MAX + 0.1);
         rightGripper.setPosition(GRIP_MIN - 0.1);
+    }
+
+    public void setLinearActuator(double steps) {
+        linearActuator.setPosition(steps);
+    }
 
+    public void setActuatorDirection(Servo.Direction direction) {
+        linearActuator.setDirection(direction);
     }
 
     public void grabAndRaise(int steps) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
index 1bb529d..5700a92 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MAX;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MIN;
 import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.INCREMENT_STEPS;
 import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.LOW_HEIGHT;
 import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.MAX_HEIGHT;
@@ -13,6 +15,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
@@ -64,6 +67,7 @@ public void runOpMode() throws InterruptedException {
             telemetry.addData("Arm Power:", slide.slideMotor.getPower());
             telemetry.addData("Arm Current (A):", slide.slideMotor.getCurrent(CurrentUnit.AMPS));
             telemetry.addData("Arm ZeroPower:", slide.slideMotor.getZeroPowerBehavior());
+            telemetry.addData("Linear Actuator Position:", slide.linearActuator.getPosition());
 
             telemetry.update();
         }
@@ -99,6 +103,16 @@ public void updateSlideByGamepad() {
         } else if (gamepad1.left_trigger > 0.5) {
             slide.ungrab();
         }
+
+        if (gamepad2.right_trigger > 0.5) {
+            slide.setActuatorDirection(Servo.Direction.FORWARD);
+            slide.setLinearActuator(ACTUATOR_MIN);
+
+        } else if (gamepad2.left_trigger > 0.5) {
+            slide.setActuatorDirection(Servo.Direction.FORWARD);
+            slide.setLinearActuator(ACTUATOR_MAX);
+
+        }
     }
 
     public void updateDriveByGamepad() {

From 90f36d7d08bf0a78fe3709c9e9f3b007b9744cd7 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 2 Mar 2023 19:15:40 -0500
Subject: [PATCH 29/43] Linear actuator cone tilter

---
 .../ftc/teamcode/subsystem/slide/LinearSlide.java          | 4 ++--
 .../firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java | 7 ++++---
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
index b260106..16e45c6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
@@ -21,8 +21,8 @@ public class LinearSlide {
     public static double GRIP_MIN = 0;
     public static double GRIP_MAX = 0.6;
 
-    public static double ACTUATOR_MIN = 0.17;
-    public static double ACTUATOR_MAX = 0.82;
+    public static double ACTUATOR_MIN = 0.3;
+    public static double ACTUATOR_MAX = 0.67;
 
     public DcMotorEx slideMotor;
     public Servo leftGripper, rightGripper;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
index 5700a92..df1e66e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TwoPlayerTeleOp.java
@@ -15,7 +15,6 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
@@ -49,6 +48,10 @@ public void runOpMode() throws InterruptedException {
 
         drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         drive.imu.resetYaw();
+        while (opModeInInit()) {
+            telemetry.addData("Linear Actuator Position:", slide.linearActuator.getPosition());
+            telemetry.update();
+        }
         waitForStart();
 
         while (opModeIsActive()) {
@@ -105,11 +108,9 @@ public void updateSlideByGamepad() {
         }
 
         if (gamepad2.right_trigger > 0.5) {
-            slide.setActuatorDirection(Servo.Direction.FORWARD);
             slide.setLinearActuator(ACTUATOR_MIN);
 
         } else if (gamepad2.left_trigger > 0.5) {
-            slide.setActuatorDirection(Servo.Direction.FORWARD);
             slide.setLinearActuator(ACTUATOR_MAX);
 
         }

From 096e66e70286245d90c4f597e1b70f8f66a54f75 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 2 Mar 2023 22:53:13 -0500
Subject: [PATCH 30/43] Cone 2

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 39 ++++++++++++-------
 .../teamcode/subsystem/slide/LinearSlide.java |  4 +-
 2 files changed, 27 insertions(+), 16 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index be1b93d..6b399bd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -1,5 +1,8 @@
 package org.firstinspires.ftc.teamcode.auto;
 
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MAX;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MIN;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -11,6 +14,7 @@
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
 import org.firstinspires.ftc.teamcode.subsystem.colorsensor.ColorSensor;
+import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
 import org.firstinspires.ftc.teamcode.subsystem.webcam.Webcam;
@@ -19,7 +23,7 @@
 @Config
 @Autonomous
 public class AutoSkeleton extends LinearOpMode {
-    public static double x = 58;
+    public static double x = -27;
     public static double y = -21;
     public static double f = 2;
 
@@ -65,23 +69,26 @@ public void runOpMode() throws InterruptedException {
                 .back(5)
                 .build();
 
-        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(-31.08, -64.54, Math.toRadians(90.00)))
+        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(-31.67, -64.77, Math.toRadians(90.00)))
                 .addDisplacementMarker(slide::grab)
-                .splineToConstantHeading(new Vector2d(-12.33, -52.49), Math.toRadians(70.00))
-                .splineToConstantHeading(new Vector2d(-12.53, -31.84), Math.toRadians(90.00))
+                .splineToConstantHeading(new Vector2d(-12.08, -55.64), Math.toRadians(90.00))
+                .splineToConstantHeading(new Vector2d(-12.27, -12.46), Math.toRadians(90.00))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .splineToConstantHeading(new Vector2d(-24.38, -7), Math.toRadians(180.00))
+                .splineToConstantHeading(new Vector2d(-29.01, -9), Math.toRadians(180.00))
                 .waitSeconds(1)
                 .build();
         drive.setPoseEstimate(trajA.start());
 //new Pose2d(-24.38, -7, Math.toRadians(90.00))
         TrajectorySequence trajB = drive.trajectorySequenceBuilder(trajA.end())
+
                 .addDisplacementMarker(slide::ungrab)
+
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
-                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
-                .addDisplacementMarker(() -> slide.setSlide(480))
-                .lineToSplineHeading(new Pose2d(-64.00, -7.78, Math.toRadians(180.00)))
+                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+                .addDisplacementMarker(() -> slide.setSlide(500))
+                .lineToSplineHeading(new Pose2d(-66.5, -12, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajC = drive.trajectorySequenceBuilder(trajB.end())
@@ -90,20 +97,22 @@ public void runOpMode() throws InterruptedException {
                     sleep(500);
                 })
                 .addDisplacementMarker(() -> {
-                    slide.setSlide(1200);
+                    slide.setSlide(600);
+                    slide.setLinearActuator(ACTUATOR_MIN);
+
                     sleep(1000);
                 })
 //                .addDisplacementMarker(() -> slide.grabAndRaise(800))
-                .lineToConstantHeading(new Vector2d(-48.61, -10.72))
-                .lineToSplineHeading(new Pose2d(-24, -10.75, Math.toRadians(90.00)))
+                .lineToSplineHeading(new Pose2d(-27, -9, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(LinearSlide.MAX_HEIGHT);
-                    sleep(1000);
+                    slide.setLinearActuator(ACTUATOR_MAX);
+                    sleep(2000);
                 })
-                .lineToConstantHeading(new Vector2d(-27, -3))
+                .lineToConstantHeading(new Vector2d(-27, -7), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.ungrab();
-                    sleep(200);
+                    sleep(500);
                 })
                 .build();
         TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
@@ -146,11 +155,13 @@ public void runOpMode() throws InterruptedException {
         //Cone 5: 0
         slide.ungrab();
         drive.imu.resetYaw();
+        slide.setLinearActuator(ACTUATOR_MAX);
         waitForStart();
 
         if (isStopRequested()) return;
 
         drive.followTrajectorySequence(trajA);
+
         drive.followTrajectorySequence(trajB);
         drive.followTrajectorySequence(trajC);
         drive.followTrajectorySequence(trajD);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
index 16e45c6..42b2093 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/slide/LinearSlide.java
@@ -21,8 +21,8 @@ public class LinearSlide {
     public static double GRIP_MIN = 0;
     public static double GRIP_MAX = 0.6;
 
-    public static double ACTUATOR_MIN = 0.3;
-    public static double ACTUATOR_MAX = 0.67;
+    public static double ACTUATOR_MIN = 0.4;
+    public static double ACTUATOR_MAX = 0.65;
 
     public DcMotorEx slideMotor;
     public Servo leftGripper, rightGripper;

From 57273fddc105300350e0c977047782ce8a18273b Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 2 Mar 2023 23:19:25 -0500
Subject: [PATCH 31/43] 1+3 auto working (overtime, slight misalign)

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 58 ++++++++++++++++---
 1 file changed, 50 insertions(+), 8 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 6b399bd..cbebeca 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -117,11 +117,13 @@ public void runOpMode() throws InterruptedException {
                 .build();
         TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
                 .addDisplacementMarker(slide::ungrab)
+
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
-                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
-                .addDisplacementMarker(() -> slide.setSlide(360))
-                .lineToSplineHeading(new Pose2d(-64.00, -7.78, Math.toRadians(180.00)))
+                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+                .addDisplacementMarker(() -> slide.setSlide(420))
+                .lineToSplineHeading(new Pose2d(-66.5, -12, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajD.end())
@@ -130,20 +132,57 @@ public void runOpMode() throws InterruptedException {
                     sleep(500);
                 })
                 .addDisplacementMarker(() -> {
-                    slide.setSlide(1200);
+                    slide.setSlide(500);
+                    slide.setLinearActuator(ACTUATOR_MIN);
+
                     sleep(1000);
                 })
 //                .addDisplacementMarker(() -> slide.grabAndRaise(800))
-                .lineToConstantHeading(new Vector2d(-48.61, -10.72))
-                .lineToSplineHeading(new Pose2d(-24, -10.75, Math.toRadians(90.00)))
+                .lineToSplineHeading(new Pose2d(-27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(LinearSlide.MAX_HEIGHT);
+                    slide.setLinearActuator(ACTUATOR_MAX);
+                    sleep(2000);
+                })
+                .lineToConstantHeading(new Vector2d(-27, -6), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
+                })
+                .build();
+        TrajectorySequence trajF = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(slide::ungrab)
+
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+
+//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
+                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+                .addDisplacementMarker(() -> slide.setSlide(250))
+                .lineToSplineHeading(new Pose2d(-66.5, -12, Math.toRadians(180.00)))
+                .build();
+//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
+        TrajectorySequence trajG = drive.trajectorySequenceBuilder(trajF.end())
+                .addDisplacementMarker(() -> {
+                    slide.grab();
+                    sleep(500);
+                })
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(400);
+                    slide.setLinearActuator(ACTUATOR_MIN);
+
                     sleep(1000);
                 })
-                .lineToConstantHeading(new Vector2d(-27, -3))
+//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
+                .lineToSplineHeading(new Pose2d(-27, -6, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+                    slide.setLinearActuator(ACTUATOR_MAX);
+                    sleep(2000);
+                })
+                .lineToConstantHeading(new Vector2d(-27, -5), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.ungrab();
-                    sleep(200);
+                    sleep(500);
                 })
                 .build();
 
@@ -166,5 +205,8 @@ public void runOpMode() throws InterruptedException {
         drive.followTrajectorySequence(trajC);
         drive.followTrajectorySequence(trajD);
         drive.followTrajectorySequence(trajE);
+
+        drive.followTrajectorySequence(trajF);
+        drive.followTrajectorySequence(trajG);
     }
 }

From e59fbc0a79463bb6366d258e50bcb599d128b7b5 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 2 Mar 2023 23:20:26 -0500
Subject: [PATCH 32/43] Possible fix for misalign

---
 .../org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java     | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index cbebeca..858fd42 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -123,7 +123,7 @@ public void runOpMode() throws InterruptedException {
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
                 //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
                 .addDisplacementMarker(() -> slide.setSlide(420))
-                .lineToSplineHeading(new Pose2d(-66.5, -12, Math.toRadians(180.00)))
+                .lineToSplineHeading(new Pose2d(-66.5, -11, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajD.end())
@@ -158,7 +158,7 @@ public void runOpMode() throws InterruptedException {
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
                 //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
                 .addDisplacementMarker(() -> slide.setSlide(250))
-                .lineToSplineHeading(new Pose2d(-66.5, -12, Math.toRadians(180.00)))
+                .lineToSplineHeading(new Pose2d(-66.5, -10, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajG = drive.trajectorySequenceBuilder(trajF.end())

From f1700432fb00a1ce062e53c26f4f53d70b5542bd Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Fri, 3 Mar 2023 18:19:52 -0500
Subject: [PATCH 33/43] scrimmage

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 51 ++++++++++++++++---
 1 file changed, 43 insertions(+), 8 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 858fd42..0eb3336 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -2,6 +2,7 @@
 
 import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MAX;
 import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MIN;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.MIN_HEIGHT;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -40,6 +41,7 @@ public void runOpMode() throws InterruptedException {
         int[] heights = {480, 360, 180, 0, 0};
         int time = 0;
 
+        SleeveDetectionNew.ParkingPosition position;
         TrajectorySequence traj1 = drive.trajectorySequenceBuilder(new Pose2d())
                 .addDisplacementMarker(slide::grab)
                 .addDisplacementMarker(() -> slide.setSlide(60))
@@ -88,7 +90,7 @@ public void runOpMode() throws InterruptedException {
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
                 //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
                 .addDisplacementMarker(() -> slide.setSlide(500))
-                .lineToSplineHeading(new Pose2d(-66.5, -12, Math.toRadians(180.00)))
+                .lineToSplineHeading(new Pose2d(-65.5, -12, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajC = drive.trajectorySequenceBuilder(trajB.end())
@@ -96,11 +98,12 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
+                .lineToConstantHeading(new Vector2d(-64.5, -12), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
-                    slide.setSlide(600);
+                    slide.setSlide(800);
                     slide.setLinearActuator(ACTUATOR_MIN);
-
                     sleep(1000);
+
                 })
 //                .addDisplacementMarker(() -> slide.grabAndRaise(800))
                 .lineToSplineHeading(new Pose2d(-27, -9, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
@@ -131,11 +134,12 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
+                .lineToConstantHeading(new Vector2d(-65.5, -11), SampleMecanumDrive.getVelocityConstraint(5, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(500);
                     slide.setLinearActuator(ACTUATOR_MIN);
 
-                    sleep(1000);
+                    sleep(1500);
                 })
 //                .addDisplacementMarker(() -> slide.grabAndRaise(800))
                 .lineToSplineHeading(new Pose2d(-27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
@@ -186,7 +190,27 @@ public void runOpMode() throws InterruptedException {
                 })
                 .build();
 
-
+        TrajectorySequence trajCenter = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(MIN_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(-45, -10))
+                .build();
+        TrajectorySequence trajRight = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(MIN_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(-12, -10))
+                .build();
+        TrajectorySequence trajLeft = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(MIN_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(-70, -10))
+                .build();
         //Cone 1: 480
         //Cone 2: 360
         //Cone 3: 180
@@ -196,17 +220,28 @@ public void runOpMode() throws InterruptedException {
         drive.imu.resetYaw();
         slide.setLinearActuator(ACTUATOR_MAX);
         waitForStart();
-
+        position = sleeveDetection.getPosition();
         if (isStopRequested()) return;
 
+
         drive.followTrajectorySequence(trajA);
 
         drive.followTrajectorySequence(trajB);
         drive.followTrajectorySequence(trajC);
+
         drive.followTrajectorySequence(trajD);
         drive.followTrajectorySequence(trajE);
 
-        drive.followTrajectorySequence(trajF);
-        drive.followTrajectorySequence(trajG);
+        switch (position) {
+            case LEFT:
+                drive.followTrajectorySequence(trajLeft);
+                break;
+            case CENTER:
+                drive.followTrajectorySequence(trajCenter);
+                break;
+            case RIGHT:
+                drive.followTrajectorySequence(trajRight);
+        }
+
     }
 }

From c94392d8a991f6d745fa9992ca642e89d56bbdf5 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 3 Mar 2023 18:46:14 -0500
Subject: [PATCH 34/43] unholy merge of old and new code

---
 .../ftc/teamcode/auto/AutoFunny.java          |   4 +-
 .../ftc/teamcode/auto/AutoSkeleton.java       |   6 +-
 .../ftc/teamcode/old/RobotOpMode.java         |  30 ++
 .../ftc/teamcode/old/auto/AutoLeft.java       | 154 ++++++++
 .../ftc/teamcode/old/auto/AutoRight.java      | 174 +++++++++
 .../ftc/teamcode/old/auto/AutoTest.java       | 201 ++++++++++
 .../ftc/teamcode/old/auto/AutoTestRight.java  | 197 ++++++++++
 .../teamcode/old/auto/AutonomousUpdated.java  |  91 +++++
 .../ftc/teamcode/old/auto/Jumpscare.java      |  27 ++
 .../old/pipeline/BoeDetectionOld.java         | 185 +++++++++
 .../old/pipeline/PoleDetectionOld.java        | 103 +++++
 .../old/pipeline/SleeveDetectionNewOld.java   |  89 +++++
 .../old/pipeline/SleeveDetectionRightOld.java | 144 +++++++
 .../teamcode/old/subsystem/LinearSlide.java   | 145 +++++++
 .../ftc/teamcode/old/subsystem/SensorC.java   |  75 ++++
 .../teamcode/old/subsystem/StrafeDrive.java   | 358 ++++++++++++++++++
 .../ftc/teamcode/old/subsystem/Subsystem.java |  13 +
 .../ftc/teamcode/old/subsystem/Webcam.java    |  40 ++
 .../ftc/teamcode/old/teleop/SensorColor.java  | 222 +++++++++++
 .../old/teleop/TwoPlayerTeleopOld.java        |  27 ++
 20 files changed, 2280 insertions(+), 5 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoLeft.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutonomousUpdated.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/Jumpscare.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetectionOld.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetectionOld.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNewOld.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRightOld.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/SensorC.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
index 1bc98ee..ec789c4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
@@ -9,7 +9,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld;
 import org.firstinspires.ftc.teamcode.subsystem.colorsensor.ColorSensor;
 import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
@@ -25,7 +25,7 @@ public void runOpMode() throws InterruptedException {
 
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
         LinearSlide slide = new LinearSlide(hardwareMap);
-        SleeveDetectionNew sleeveDetection = new SleeveDetectionNew();
+        SleeveDetectionNewOld sleeveDetection = new SleeveDetectionNewOld();
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
         ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 0eb3336..925bfb6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -13,7 +13,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pipeline.SleeveDetectionNew;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld;
 import org.firstinspires.ftc.teamcode.subsystem.colorsensor.ColorSensor;
 import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
@@ -34,14 +34,14 @@ public void runOpMode() throws InterruptedException {
 
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
         LinearSlide slide = new LinearSlide(hardwareMap);
-        SleeveDetectionNew sleeveDetection = new SleeveDetectionNew();
+        SleeveDetectionNewOld sleeveDetection = new SleeveDetectionNewOld();
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
         ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
         int[] heights = {480, 360, 180, 0, 0};
         int time = 0;
 
-        SleeveDetectionNew.ParkingPosition position;
+        SleeveDetectionNewOld.ParkingPosition position;
         TrajectorySequence traj1 = drive.trajectorySequenceBuilder(new Pose2d())
                 .addDisplacementMarker(slide::grab)
                 .addDisplacementMarker(() -> slide.setSlide(60))
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
new file mode 100644
index 0000000..255ac0c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/RobotOpMode.java
@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.old;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.subsystem.Subsystem;
+
+abstract public class RobotOpMode extends LinearOpMode {
+    public void addSubsystems(Subsystem... subsystems) {
+        for (Subsystem subsystem : subsystems) {
+            subsystem.setup();
+        }
+    }
+
+    // Waits until at least one device or OpMode is not busy
+    public void blockOn(DcMotor... motors) {
+        while (opModeIsActive() && isBusy(motors)) {
+            idle();
+        }
+    }
+
+    public boolean isBusy(DcMotor... motors) {
+        for (DcMotor motor : motors) {
+            if (!motor.isBusy()) {
+                return false;
+            }
+        }
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoLeft.java
new file mode 100644
index 0000000..a161bd6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoLeft.java
@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode.old.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRightOld;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRightOld.ParkingPosition;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
+
+@Disabled
+//@Autonomous(name = "Autonomous Left")
+public class AutoLeft extends RobotOpMode {
+    private final StrafeDrive drive = new StrafeDrive(this);
+    private final LinearSlide slide = new LinearSlide(this);
+    private final SleeveDetectionRightOld sleeve = new SleeveDetectionRightOld();
+    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
+
+    @Override
+    public void runOpMode() {
+        addSubsystems(drive, slide, cam);
+
+        // Put motors in encoder mode
+        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide.release();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+        ParkingPosition position = sleeve.getPosition();
+        //Place First Cone
+        slide.grab();
+        drive.driveInches(5.5, 5.5, 0.2f);
+        slide.setSlideAndWait(800);
+        drive.strafeInches(38.5, 0.5f);
+        slide.setSlideAndWait(1900);
+        drive.driveInches(6.5, 6.5, 0.2f);
+        slide.release();
+
+        //Go for Second Cone
+        drive.strafeInches(13.0, 0.5f);
+        slide.setSlideAndWait(500);
+        drive.driveInches(20, 20, 0.5f);
+        slide.grab();
+        slide.setSlideAndWait(1000);
+        drive.driveInches(-1, -1, 0.25f);
+        slide.setSlideAndWait(1500);
+        drive.driveInches(-5, -5, 0.25f);
+        slide.setSlideAndWait(300);
+        drive.driveInches(-44, -44, 0.5f);
+        drive.strafeInches(-12.5, 0.5f);
+        slide.setSlideAndWait(3200);
+        drive.driveInches(4, 4, 0.2f);
+        slide.release();
+        drive.driveInches(-4, -4, 0.2f);
+        slide.setSlideAndWait(280);
+        drive.driveInches(0, 1.5, 0.5f);
+        drive.strafeInches(12, 0.5f);
+
+        //Third Cone
+//        drive.driveInches(50, 50, 0.3f);
+//        slide.grab();
+//        slide.setSlide(150, true);
+//        drive.driveInches(-1, -1, 0.25f);
+//        slide.setSlide(375, true);
+//        drive.driveInches(-5, -5, 0.25f);
+//        slide.setSlide(50, true);
+//        drive.driveInches(-38, -38, 0.5f);
+//        slide.setSlide(1000, true);
+//        drive.drive(700, -700, 0.2f);
+//        drive.driveInches(4, 4, 0.2f);
+//        slide.ungrab();
+//        drive.driveInches(-4, -4, 0.2f);
+//        drive.drive(-700, 700, 0.2f);
+
+        //Move to Parking
+        switch (position) {
+            case CENTER:
+                drive.driveInches(24, 24, 0.2f);
+                break;
+            case RIGHT:
+                break;
+            case LEFT:
+                drive.driveInches(48, 48, 0.2f);
+                break;
+        }
+        slide.setSlideAndWait(0);
+        /*
+        1350 is Approximately 25 inches moving forwards
+        1350 is 24 inches moving sideways
+        270 rotation is approximately 35 degrees
+        250 linear slide is 1.5 inches
+
+        Directions
+        strafe 39.5 inches
+        place cone
+        strafe 12 inches
+        rotate 180 degrees
+        move forwards 27 inches
+        */
+
+        //        robot.linearSlide.grab();
+//        drive.driveInches(3, 3, 0.25f);
+//        drive.strafeInches(39.5, 0.25f);
+
+//        drive.drive(200, 200, 0.25f);
+
+        //place first cone (low)
+
+//        robot.linearSlide.unGrab();
+
+//        drive.drive(-200, -200, 0.25f);zs
+//        slide.setSlide(0);
+//        drive.strafeInches(20, 0.25f);
+//        robot.strafeDrive.drive(-926, 926, 0.5f);
+//        drive.driveInches(27, 27, 0.25f);
+        //grab second cone
+//        slide.setSlide(300);
+
+//        robot.linearSlide.grab();
+
+//        slide.setSlide(450);
+//        drive.driveInches(-3, -3, 0.25f);
+//        slide.setSlide(50);
+
+//        drive.driveInches(-37, -37, 0.25f);
+//        drive.strafeInches(-10, 0.5f);
+//        drive.drive(200, 200, 0.5f);
+        //place second cone (mid)
+
+//        slide.setSlide(300);
+//        drive.drive(-200, -200, 0.5f);
+//        drive.strafeInches(10, 0.5f);
+//        drive.driveInches(40, 40, 0.5f);
+
+        //grab third cone
+
+//        drive.driveInches(-35, -35, 0.5f);
+//        drive.drive(-463, 463, 0.5f);
+
+//        place third cone
+
+//        drive.drive(463, -463, 0.5f);
+        //park
+
+//        robot.strafeDrive.drive(1680, 1680, 0.5f);
+//        robot.strafeDrive.strafe(1000, 0.5f);
+//        robot.strafeDrive.strafe(-1000, 0.5f);
+//        robot.strafeDrive.drive(1000, -1000, 0.5f);
+//        robot.strafeDrive.drive(-520,-520, 0.5f);
+//        robot.strafeDrive.drive(-520,-520, -0.5f);
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
new file mode 100644
index 0000000..7d99c57
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoRight.java
@@ -0,0 +1,174 @@
+package org.firstinspires.ftc.teamcode.old.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRightOld;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRightOld.ParkingPosition;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
+
+@Disabled
+//@Autonomous(name = "Autonomous Right")
+public class AutoRight extends RobotOpMode {
+    private final StrafeDrive drive = new StrafeDrive(this);
+    private final LinearSlide slide = new LinearSlide(this);
+    private final SleeveDetectionRightOld sleeve = new SleeveDetectionRightOld();
+    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
+
+    @Override
+    public void runOpMode() {
+        addSubsystems(drive, slide, cam);
+
+        // Put motors in encoder mode
+        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide.release();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+        ParkingPosition position = sleeve.getPosition();
+        //Place First Cone
+        slide.grab();
+        drive.driveInches(5.5, 5.5, 0.2f);
+        slide.setSlideAndWait(800);
+        drive.strafeInches(-37.5, 0.5f);
+        slide.setSlideAndWait(1900);
+        drive.driveInches(6.5, 6.5, 0.2f);
+        slide.release();
+
+        //Go for Second Cone
+        drive.strafeInches(-14.0, 0.5f);
+        slide.setSlideAndWait(500);
+        drive.driveInches(20, 20, 0.5f);
+        slide.grab();
+        slide.setSlideAndWait(1000);
+        drive.driveInches(-1, -1, 0.25f);
+        slide.setSlideAndWait(1500);
+        drive.driveInches(-5, -5, 0.25f);
+        slide.setSlideAndWait(300);
+        drive.driveInches(-44, -44, 0.5f);
+        drive.strafeInches(12.5, 0.5f);
+        slide.setSlideAndWait(3200);
+        drive.driveInches(4, 4, 0.2f);
+        slide.release();
+        drive.driveInches(-4, -4, 0.2f);
+        slide.setSlideAndWait(280);
+        drive.driveInches(0, 1.5, 0.5f);
+        drive.strafeInches(-12, 0.5f);
+
+
+        //Third Cone
+//        drive.driveInches(50, 50, 0.3f);
+//        slide.grab();
+//        slide.setSlide(150, true);
+//        drive.driveInches(-1, -1, 0.25f);
+//        slide.setSlide(375, true);
+//        drive.driveInches(-5, -5, 0.25f);
+//        slide.setSlide(50, true);
+//        drive.driveInches(-38, -38, 0.5f);
+//        slide.setSlide(1000, true);
+//        drive.drive(700, -700, 0.2f);
+//        drive.driveInches(4, 4, 0.2f);
+//        slide.ungrab();
+//        drive.driveInches(-4, -4, 0.2f);
+//        drive.drive(-700, 700, 0.2f);
+
+        drive.driveInches(20, 20, 0.5f);
+        slide.grab();
+        slide.setSlideAndWait(250);
+        drive.driveInches(-1, -1, 0.25f);
+        slide.setSlideAndWait(375);
+        drive.driveInches(-5, -5, 0.25f);
+        slide.setSlideAndWait(75);
+        drive.driveInches(-44, -44, 0.5f);
+        drive.strafeInches(12.5, 0.5f);
+        slide.setSlideAndWait(800);
+        drive.driveInches(4, 4, 0.2f);
+        slide.release();
+        drive.driveInches(-4, -4, 0.2f);
+        slide.setSlideAndWait(70);
+        drive.driveInches(0, 1.5, 0.5f);
+        drive.strafeInches(-12, 0.5f);
+
+
+        //Move to Parking
+        switch (position) {
+
+            case CENTER:
+                drive.driveInches(24, 24, 0.2f);
+                break;
+            case RIGHT:
+                drive.driveInches(48, 48, 0.2f);
+                break;
+            case LEFT:
+                break;
+        }
+        slide.setSlideAndWait(0);
+        /*
+        1350 is Approximately 25 inches moving forwards
+        1350 is 24 inches moving sideways
+        270 rotation is approximately 35 degrees
+        250 linear slide is 1.5 inches
+
+        Directions
+        strafe 39.5 inches
+        place cone
+        strafe 12 inches
+        rotate 180 degrees
+        move forwards 27 inches
+        */
+
+        //        robot.linearSlide.grab();
+//        drive.driveInches(3, 3, 0.25f);
+//        drive.strafeInches(39.5, 0.25f);
+
+//        drive.drive(200, 200, 0.25f);
+
+        //place first cone (low)
+
+//        robot.linearSlide.unGrab();
+
+//        drive.drive(-200, -200, 0.25f);zs
+//        slide.setSlide(0);
+//        drive.strafeInches(20, 0.25f);
+//        robot.strafeDrive.drive(-926, 926, 0.5f);
+//        drive.driveInches(27, 27, 0.25f);
+        //grab second cone
+//        slide.setSlide(300);
+
+//        robot.linearSlide.grab();
+
+//        slide.setSlide(450);
+//        drive.driveInches(-3, -3, 0.25f);
+//        slide.setSlide(50);
+
+//        drive.driveInches(-37, -37, 0.25f);
+//        drive.strafeInches(-10, 0.5f);
+//        drive.drive(200, 200, 0.5f);
+        //place second cone (mid)
+
+//        slide.setSlide(300);
+//        drive.drive(-200, -200, 0.5f);
+//        drive.strafeInches(10, 0.5f);
+//        drive.driveInches(40, 40, 0.5f);
+
+        //grab third cone
+
+//        drive.driveInches(-35, -35, 0.5f);
+//        drive.drive(-463, 463, 0.5f);
+
+//        place third cone
+
+//        drive.drive(463, -463, 0.5f);
+        //park
+
+//        robot.strafeDrive.drive(1680, 1680, 0.5f);
+//        robot.strafeDrive.strafe(1000, 0.5f);
+//        robot.strafeDrive.strafe(-1000, 0.5f);
+//        robot.strafeDrive.drive(1000, -1000, 0.5f);
+//        robot.strafeDrive.drive(-520,-520, 0.5f);
+//        robot.strafeDrive.drive(-520,-520, -0.5f);
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTest.java
new file mode 100644
index 0000000..d446765
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTest.java
@@ -0,0 +1,201 @@
+package org.firstinspires.ftc.teamcode.old.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.PoleDetectionOld;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld.ParkingPosition;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
+
+@Disabled
+@Autonomous(name = "Autonomous Left")
+public class AutoTest extends RobotOpMode {
+    private final StrafeDrive drive = new StrafeDrive(this);
+    private final LinearSlide slide = new LinearSlide(this);
+    private final SleeveDetectionNewOld sleeve = new SleeveDetectionNewOld();
+    private final PoleDetectionOld pole = new PoleDetectionOld();
+    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
+
+    @Override
+    public void runOpMode() {
+        addSubsystems(drive, slide, cam);
+
+        // Put motors in encoder mode
+        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide.release();
+
+        drive.imu.resetYaw();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+        drive.imu.resetYaw();
+        ParkingPosition position = sleeve.getPosition();
+        slide.grabAndWait();
+        slide.setSlide(200);
+        drive.driveInches(2, 2, 0.75f);
+//        drive.drive(50,-50, 0.75f);
+        drive.strafeInches(20, 0.6f);
+        drive.driveInches(55, 55, 0.75f);
+//        drive.strafeInches(-12, 0.5f);
+        slide.setSlideAndWait(4000);
+        drive.drive(-375, 375, 0.35f);
+        drive.driveInches(11, 11, 0.2f);
+        drive.driveInches(-4, -4, 0.2f);
+        slide.setSlideAndWait(3250);
+        slide.release();
+
+
+        slide.setSlideAndWait(4000);
+        drive.driveInches(-5, -5, 0.2f);
+        slide.setSlide(600);
+
+//        drive.driveInches(-2250,-2250, 0.5f);
+        //Second CONEEeee
+        drive.drive(-450, 450, 0.35f);
+        drive.strafeInches(-4, 0.6f);
+        drive.driveInches(48, 48, 0.75f);
+//        drive.strafeInches(4, 0.5f);
+//        drive.driveInches(10,10,0.25f);
+        drive.driveInches(-2, -2, 0.5f);
+        slide.grabAndWait();
+        slide.setSlideAndWait(1400);
+        drive.driveInches(-5, -5, 0.25f);
+        drive.drive(-50, 50, 0.25f);
+//        drive.strafeInches(-4,0.25f);
+        slide.setSlide(460);
+        drive.driveInches(-16, -16, 0.75f);
+        drive.driveInches(-16, -16, 0.75f);
+        drive.drive(850, -850, 0.25f);
+        slide.setSlideAndWait(4000);
+        drive.driveInches(8, 8, 0.2f);
+        drive.driveInches(-2, -2, 0.2f);
+        slide.setSlideAndWait(3250);
+        slide.release();
+        slide.setSlideAndWait(4000);
+        drive.driveInches(-3, -3, 0.2f);
+        slide.setSlideAndWait(0);
+        //Stuff Work
+
+        //THIRD CONEEEEE
+//        slide.setSlide(300,true);
+//        drive.drive(-725,725,0.35f);
+//        drive.driveInches(21,21,0.5f);
+//        drive.driveInches(16,16,0.5f);
+//        slide.grab();
+//        drive.driveInches(2,2,0.35f);
+//        slide.setSlide(1500,true);
+//        drive.driveInches(-3,-3,0.25f);
+//        slide.setSlide(420,true);
+//        drive.strafeInches(1,0.35f);
+//        drive.driveInches(-32,-32,0.75f);
+//        drive.drive(725,-725,0.25f);
+//        slide.setSlide(4000, true);
+//        drive.driveInches(2,2, 0.2f);
+//        slide.setSlide(3250, true);
+//        slide.ungrab();
+//        drive.driveInches(-2,-2,0.2f);
+//        slide.setSlide(0, true);
+
+        //GAS GAS GAS
+//        drive.driveInches(2,2,1f);
+//        drive.strafeInches(18,0.75f);
+////        drive.turnRight(0, 0.2f);
+//        drive.driveInches(50, 50,0.75f);
+//        drive.strafeInches(-12, 0.5f);
+////        drive.turnRight(0, 0.2f);
+////        cam.camera.setPipeline(pole);
+////        while(!pole.getPole()) {
+////            drive.strafeInches(-2, 0.5f);
+////        }
+//        slide.setSlide(4000, true);
+//        drive.driveInches(4,4, 0.2f);
+//        slide.setSlide(3250, true);
+//        slide.ungrab();
+//        drive.driveInches(-5,-5,0.2f);
+//        slide.setSlide(600, true);
+//
+////        drive.driveInches(-2250,-2250, 0.5f);
+//        //Second CONEEeee
+//        drive.drive(-775,775,0.35f);
+//        drive.driveInches(21,21,1f);
+//        drive.strafeInches(4, 0.5f);
+//        drive.driveInches(16,16,0.5f);
+//        slide.grab();
+//        drive.driveInches(5,5,0.2f);
+//        slide.setSlide(1500,true);
+//        drive.driveInches(-5,-5,0.25f);
+//        slide.setSlide(460,true);
+//        drive.strafeInches(2,0.35f);
+//        drive.driveInches(-16,-16,0.75f);
+//        drive.strafeInches(-2,1f);
+//        drive.driveInches(-16,-16,0.75f);
+//        drive.drive(725,-725,0.25f);
+//        slide.setSlide(4000, true);
+//        drive.driveInches(1,1, 0.2f);
+//        slide.setSlide(3250, true);
+//        slide.ungrab();
+//        drive.driveInches(-3,-3,0.2f);
+//        //Stuff Work
+//
+//        //THIRD CONEEEEE
+////        slide.setSlide(300,true);
+////        drive.drive(-725,725,0.35f);
+////        drive.driveInches(21,21,0.5f);
+////        drive.driveInches(16,16,0.5f);
+////        slide.grab();
+////        drive.driveInches(2,2,0.35f);
+////        slide.setSlide(1500,true);
+////        drive.driveInches(-3,-3,0.25f);
+////        slide.setSlide(420,true);
+////        drive.strafeInches(1,0.35f);
+////        drive.driveInches(-32,-32,0.75f);
+////        drive.drive(725,-725,0.25f);
+////        slide.setSlide(4000, true);
+////        drive.driveInches(2,2, 0.2f);
+////        slide.setSlide(3250, true);
+////        slide.ungrab();
+////        drive.driveInches(-2,-2,0.2f);
+////        slide.setSlide(0, true);
+
+        //Stuff No Work Yet
+        switch (position) {
+            case CENTER:
+                drive.strafeInches(-15, 0.2f);
+                break;
+            case RIGHT:
+                drive.strafeInches(14, 0.2f);
+                break;
+            case LEFT:
+                drive.strafeInches(-38, 0.2f);
+                break;
+        }
+
+//        cycle(700);
+    }
+
+    public void cycle(int height) {
+        drive.drive(-850, 850, 0.25f);
+        drive.driveInches(33, 33, 0.45f);
+        slide.setSlideAndWait(height);
+        drive.driveInches(4, 4, 0.25f);
+        slide.grab();
+        drive.driveInches(-1, -1, 0.15f);
+        slide.setSlideAndWait(height + 600);
+//        drive.driveInches(-2, -2,0.25f);
+//        slide.setSlide(900,true);
+        drive.driveInches(-13, -10, 0.25f);
+        slide.setSlideAndWait(300);
+        drive.driveInches(-25, -25, 0.45f);
+        drive.drive(875, -875, 0.25f);
+        slide.setSlideAndWait(4150);
+        drive.driveInches(2, 2, 0.15f);
+        slide.release();
+        drive.driveInches(-2, -2, 0.15f);
+        slide.setSlideAndWait(0);
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
new file mode 100644
index 0000000..5e38429
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutoTestRight.java
@@ -0,0 +1,197 @@
+package org.firstinspires.ftc.teamcode.old.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.PoleDetectionOld;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld.ParkingPosition;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
+
+@Disabled
+@Autonomous(name = "Autonomous Right")
+public class AutoTestRight extends RobotOpMode {
+    private final StrafeDrive drive = new StrafeDrive(this);
+    private final LinearSlide slide = new LinearSlide(this);
+    private final SleeveDetectionNewOld sleeve = new SleeveDetectionNewOld();
+    private final PoleDetectionOld pole = new PoleDetectionOld();
+    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
+
+    @Override
+    public void runOpMode() {
+        addSubsystems(drive, slide, cam);
+
+        // Put motors in encoder mode
+        drive.setModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide.release();
+
+        drive.imu.resetYaw();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+        drive.imu.resetYaw();
+        ParkingPosition position = sleeve.getPosition();
+        slide.grab();
+        slide.setSlide(200);
+        drive.driveInches(2, 2, 0.75f);
+//        drive.drive(50,-50, 0.75f);
+        drive.strafeInches(-20, 0.6f);
+        drive.driveInches(55, 55, 0.75f);
+//        drive.strafeInches(-12, 0.5f);
+        slide.setSlideAndWait(4000);
+        drive.drive(375, -375, 0.35f);
+        drive.driveInches(11, 11, 0.2f);
+        drive.driveInches(-4, -4, 0.2f);
+        slide.setSlideAndWait(3250);
+        slide.release();
+        slide.setSlideAndWait(4000);
+        drive.driveInches(-5, -5, 0.2f);
+        slide.setSlide(600);
+//        drive.driveInches(-2250,-2250, 0.5f);
+        //Second CONEEeee
+        drive.drive(450, -450, 0.35f);
+        drive.strafeInches(2, 0.6f);
+        drive.driveInches(48, 48, 0.75f);
+//        drive.strafeInches(4, 0.5f);
+//        drive.driveInches(10,10,0.25f);
+        drive.driveInches(-2, -2, 0.5f);
+        slide.grab();
+        slide.setSlideAndWait(1400);
+        drive.driveInches(-5, -5, 0.25f);
+        drive.drive(50, -50, 0.25f);
+//        drive.strafeInches(-4,0.25f);
+        slide.setSlide(460);
+        drive.driveInches(-16, -16, 0.75f);
+        drive.driveInches(-16, -16, 0.75f);
+        drive.drive(-850, 850, 0.25f);
+        slide.setSlideAndWait(4000);
+        drive.driveInches(8, 8, 0.2f);
+        drive.driveInches(-2, -2, 0.2f);
+        slide.setSlideAndWait(3250);
+        slide.release();
+        slide.setSlideAndWait(4000);
+        drive.driveInches(-3, -3, 0.2f);
+        slide.setSlideAndWait(0);
+        //Stuff Work
+
+        //THIRD CONEEEEE
+//        slide.setSlide(300,true);
+//        drive.drive(-725,725,0.35f);
+//        drive.driveInches(21,21,0.5f);
+//        drive.driveInches(16,16,0.5f);
+//        slide.grab();
+//        drive.driveInches(2,2,0.35f);
+//        slide.setSlide(1500,true);
+//        drive.driveInches(-3,-3,0.25f);
+//        slide.setSlide(420,true);
+//        drive.strafeInches(1,0.35f);
+//        drive.driveInches(-32,-32,0.75f);
+//        drive.drive(725,-725,0.25f);
+//        slide.setSlide(4000, true);
+//        drive.driveInches(2,2, 0.2f);
+//        slide.setSlide(3250, true);
+//        slide.ungrab();
+//        drive.driveInches(-2,-2,0.2f);
+//        slide.setSlide(0, true);
+
+        //GAS GAS GAS
+//        drive.driveInches(2,2,1f);
+//        drive.strafeInches(18,0.75f);
+////        drive.turnRight(0, 0.2f);
+//        drive.driveInches(50, 50,0.75f);
+//        drive.strafeInches(-12, 0.5f);
+////        drive.turnRight(0, 0.2f);
+////        cam.camera.setPipeline(pole);
+////        while(!pole.getPole()) {
+////            drive.strafeInches(-2, 0.5f);
+////        }
+//        slide.setSlide(4000, true);
+//        drive.driveInches(4,4, 0.2f);
+//        slide.setSlide(3250, true);
+//        slide.ungrab();
+//        drive.driveInches(-5,-5,0.2f);
+//        slide.setSlide(600, true);
+//
+////        drive.driveInches(-2250,-2250, 0.5f);
+//        //Second CONEEeee
+//        drive.drive(-775,775,0.35f);
+//        drive.driveInches(21,21,1f);
+//        drive.strafeInches(4, 0.5f);
+//        drive.driveInches(16,16,0.5f);
+//        slide.grab();
+//        drive.driveInches(5,5,0.2f);
+//        slide.setSlide(1500,true);
+//        drive.driveInches(-5,-5,0.25f);
+//        slide.setSlide(460,true);
+//        drive.strafeInches(2,0.35f);
+//        drive.driveInches(-16,-16,0.75f);
+//        drive.strafeInches(-2,1f);
+//        drive.driveInches(-16,-16,0.75f);
+//        drive.drive(725,-725,0.25f);
+//        slide.setSlide(4000, true);
+//        drive.driveInches(1,1, 0.2f);
+//        slide.setSlide(3250, true);
+//        slide.ungrab();
+//        drive.driveInches(-3,-3,0.2f);
+//        //Stuff Work
+//
+//        //THIRD CONEEEEE
+////        slide.setSlide(300,true);
+////        drive.drive(-725,725,0.35f);
+////        drive.driveInches(21,21,0.5f);
+////        drive.driveInches(16,16,0.5f);
+////        slide.grab();
+////        drive.driveInches(2,2,0.35f);
+////        slide.setSlide(1500,true);
+////        drive.driveInches(-3,-3,0.25f);
+////        slide.setSlide(420,true);
+////        drive.strafeInches(1,0.35f);
+////        drive.driveInches(-32,-32,0.75f);
+////        drive.drive(725,-725,0.25f);
+////        slide.setSlide(4000, true);
+////        drive.driveInches(2,2, 0.2f);
+////        slide.setSlide(3250, true);
+////        slide.ungrab();
+////        drive.driveInches(-2,-2,0.2f);
+////        slide.setSlide(0, true);
+
+        //Stuff No Work Yet
+        switch (position) {
+            case CENTER:
+                drive.strafeInches(14, 0.2f);
+                break;
+            case RIGHT:
+                drive.strafeInches(38, 0.2f);
+                break;
+            case LEFT:
+                drive.strafeInches(-12, 0.2f);
+                break;
+        }
+
+//        cycle(700);
+    }
+
+    public void cycle(int height) {
+        drive.drive(-850, 850, 0.25f);
+        drive.driveInches(33, 33, 0.45f);
+        slide.setSlideAndWait(height);
+        drive.driveInches(4, 4, 0.25f);
+        slide.grab();
+        drive.driveInches(-1, -1, 0.15f);
+        slide.setSlideAndWait(height + 600);
+//        drive.driveInches(-2, -2,0.25f);
+//        slide.setSlide(900,true);
+        drive.driveInches(-13, -10, 0.25f);
+        slide.setSlideAndWait(300);
+        drive.driveInches(-25, -25, 0.45f);
+        drive.drive(875, -875, 0.25f);
+        slide.setSlideAndWait(4150);
+        drive.driveInches(2, 2, 0.15f);
+        slide.release();
+        drive.driveInches(-2, -2, 0.15f);
+        slide.setSlideAndWait(0);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutonomousUpdated.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutonomousUpdated.java
new file mode 100644
index 0000000..3fb075b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/AutonomousUpdated.java
@@ -0,0 +1,91 @@
+package org.firstinspires.ftc.teamcode.old.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.PoleDetectionOld;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.SensorC;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
+
+@Disabled
+@Autonomous(name = "Autonomous Gyro Test Left")
+public class AutonomousUpdated extends RobotOpMode {
+    private final StrafeDrive drive = new StrafeDrive(this);
+    private final LinearSlide slide = new LinearSlide(this);
+    private final SleeveDetectionNewOld sleeve = new SleeveDetectionNewOld();
+    private final PoleDetectionOld pole = new PoleDetectionOld();
+    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
+    private final SensorC colorsensor = new SensorC(this);
+
+    @Override
+    public void runOpMode() {
+        addSubsystems(drive, slide, cam, colorsensor);
+
+
+        // Put motors in encoder mode
+
+        slide.release();
+        while (opModeInInit()) {
+            drive.updateHeadingDeg();
+            telemetry.addData("Heading: ", drive.botHeading);
+            telemetry.update();
+        }
+        waitForStart();
+        drive.imu.resetYaw();
+//        slide.grabAndWait();
+//        slide.setSlide(200);
+//        drive.strafeInches(20, 0.5f);
+//        drive.rotateAndMove(4.5, 45, 0.3, 0, -0.3);
+        drive.rotateAndMoveInches(0, 2, 20, 0.5, 0);
+        drive.rotateAndMoveInches(0, 57, 0, 0.5, 0);
+        slide.setSlideAndWait(4000);
+        drive.rotateAndMoveInches(0, 0, -12, 0.5, 0);
+        doTheFunny(drive, slide, 490);
+        doTheFunny(drive, slide, 425);
+    }
+
+    public static void doTheFunny(StrafeDrive drive, LinearSlide slide, int height) {
+        toStack(drive);
+        coneFromStack(drive, slide, height);
+    }
+
+    public static void toStack(StrafeDrive drive) {
+        drive.rotateAndMove(3.05, 77, 0, -0.3, -0.3);
+//        drive.rotateAndMoveInches(77,0,45,0.3,0.3);
+//        drive.rotateAndMove(0.25, 75, 0.3, 0, 0);
+    }
+
+    public static void coneFromStack(StrafeDrive drive, LinearSlide slide, int height) {
+        slide.setSlideAndWait(height);
+        slide.grabAndWait();
+        slide.setSlideAndWait(height + 800);
+        drive.rotateAndMove(0.95, 0, 0, 0.3, 0);
+        //drive.driveInches(-8, -8, 0.3f);
+        slide.setSlide(400);
+        drive.rotateAndMove(2.32, 7, 0, 0.3, 0.3);
+        slide.setSlideAndWait(3870);
+        drive.driveInches(6, 6, 0.15f);
+//        drive.driveInches(-2, -2, 0.15f);
+        slide.releaseAndWait();
+//        drive.driveInches(-1, -1, 0.15f);
+        slide.setSlide(200);
+//        drive.rotateAndMove(2.5, 0, 0.3, 0, -0.3);
+//        slide.grab();
+//        slide.setSlide(height + 75);
+//        drive.driveStraight(0.5f, -4, 0);
+//        drive.rotateAndMove(4.5,90, 0.3, 0, -0.3);
+    }
+
+    public void lineUpLarry(double color, float speed) {
+        drive.setModes(DcMotor.RunMode.RUN_USING_ENCODER);
+        while (colorsensor.getHue() < (color - 10) || colorsensor.getHue() > (color + 10)) {
+            drive.driveBot(0, 0.3, 0);
+        }
+        drive.setPowers(0);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/Jumpscare.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/Jumpscare.java
new file mode 100644
index 0000000..f9c63d0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/auto/Jumpscare.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.old.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionRightOld;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.subsystem.Webcam;
+
+@Disabled
+@Autonomous(name = "Jumpscare")
+public class Jumpscare extends RobotOpMode {
+    private final StrafeDrive drive = new StrafeDrive(this);
+    private final LinearSlide slide = new LinearSlide(this);
+    private final SleeveDetectionRightOld sleeve = new SleeveDetectionRightOld();
+    private final Webcam cam = new Webcam(this, "Webcam 1", sleeve);
+
+    @Override
+    public void runOpMode() {
+        addSubsystems(drive, slide, cam);
+        waitForStart();
+        drive.strafeInches(5, 0.5f);
+        drive.driveInches(-6, -6, 0.5f);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetectionOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetectionOld.java
new file mode 100644
index 0000000..979662c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/BoeDetectionOld.java
@@ -0,0 +1,185 @@
+package org.firstinspires.ftc.teamcode.old.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class BoeDetectionOld extends OpenCvPipeline {
+    /*
+    YELLOW  = Parking Left
+    CYAN    = Parking Middle
+    MAGENTA = Parking Right
+     */
+
+    public enum Cone {
+        RED,
+        BLUE,
+        UNDECIDED
+    }
+
+    public enum Distance {
+        CLOSE,
+        MID,
+        FAR,
+        UNSEEN
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static final int X = 55;
+    private static final int Y = 120;
+    private static final int STEP = 10;
+    private static final int WIDTH = 25;
+    private static final int HEIGHT = 15;
+    private static final Point FAR_TOPLEFT_ANCHOR_POINT = new Point(X, Y);
+    private static final Point CLOSE_TOPLEFT_ANCHOR_POINT = new Point(X - (2 * STEP), Y - (2 * STEP));
+
+    // Width and height for the bounding box
+    public static int FAR_REGION_WIDTH = WIDTH;
+    public static int FAR_REGION_HEIGHT = HEIGHT;
+    public static int MID_REGION_WIDTH = WIDTH + STEP + STEP;
+    public static int MID_REGION_HEIGHT = HEIGHT + STEP + STEP;
+    public static int CLOSE_REGION_WIDTH = WIDTH + STEP + STEP + STEP + STEP;
+    public static int CLOSE_REGION_HEIGHT = HEIGHT + STEP + STEP + STEP + STEP;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_red_bounds = new Scalar(100, 0, 0, 255),
+            upper_red_bounds = new Scalar(255, 150, 150, 255),
+            lower_blue_bounds = new Scalar(0, 0, 100, 255),
+            upper_blue_bounds = new Scalar(150, 150, 255, 255);
+
+    // Color definitions
+    private final Scalar
+            RED = new Scalar(255, 0, 0),
+            BLUE = new Scalar(0, 0, 255),
+            BLACK = new Scalar(0, 0, 0);
+
+    // Percent and mat definitions
+    private double redPercentC, bluePercentC, redPercentM, bluePercentM, redPercentF, bluePercentF;
+    private final Mat redMatC = new Mat();
+    private final Mat blueMatC = new Mat();
+    private Mat redMatF = new Mat();
+    private Mat blueMatF = new Mat();
+    private Mat redMatM = new Mat();
+    private Mat blueMatM = new Mat();
+    private Mat blurredMatClose = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point((2 * STEP), (2 * STEP));
+    Point sleeve_pointB = new Point(
+            (2 * STEP) + FAR_REGION_WIDTH,
+            (2 * STEP) + FAR_REGION_HEIGHT);
+
+    Point sleeve_pointC = new Point(STEP, STEP);
+    Point sleeve_pointD = new Point(
+            STEP + MID_REGION_WIDTH,
+            STEP + MID_REGION_HEIGHT);
+
+    Point sleeve_pointE = new Point(
+            CLOSE_TOPLEFT_ANCHOR_POINT.x,
+            CLOSE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointF = new Point(
+            CLOSE_TOPLEFT_ANCHOR_POINT.x + CLOSE_REGION_WIDTH,
+            CLOSE_TOPLEFT_ANCHOR_POINT.y + CLOSE_REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile Cone cone = Cone.UNDECIDED;
+    private volatile Distance distance = Distance.UNSEEN;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMatClose, new Size(5, 5));
+        blurredMatClose = blurredMatClose.submat(new Rect(sleeve_pointE, sleeve_pointF));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMatClose, blurredMatClose, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMatClose, lower_red_bounds, upper_red_bounds, redMatC);
+        Core.inRange(blurredMatClose, lower_blue_bounds, upper_blue_bounds, blueMatC);
+
+        redMatM = redMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
+        blueMatM = blueMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
+
+        redMatF = redMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
+        blueMatF = blueMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+
+        // Gets color specific values
+        redPercentC = Core.countNonZero(redMatC);
+        bluePercentC = Core.countNonZero(blueMatC);
+
+        redPercentM = Core.countNonZero(redMatM);
+        bluePercentM = Core.countNonZero(blueMatM);
+
+        redPercentF = Core.countNonZero(redMatF);
+        bluePercentF = Core.countNonZero(blueMatF);
+
+
+        // Calculates the highest amount of pixels being covered on each side
+        double maxPercent = Math.max(redPercentF, bluePercentF);
+
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (maxPercent == redPercentF) {
+            if (redPercentC >= 150) {
+                cone = Cone.RED;
+                distance = Distance.CLOSE;
+            } else if (redPercentM >= 100) {
+                cone = Cone.RED;
+                distance = Distance.MID;
+            } else if (redPercentF >= 50) {
+                cone = Cone.RED;
+                distance = Distance.FAR;
+            } else {
+                cone = Cone.UNDECIDED;
+                distance = Distance.UNSEEN;
+            }
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    RED,
+                    2
+            );
+        } else if (maxPercent == bluePercentC && bluePercentC >= 50) {
+            cone = Cone.BLUE;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    BLUE,
+                    2
+            );
+        } else {
+            cone = Cone.UNDECIDED;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    BLACK,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        redMatC.release();
+        blueMatC.release();
+        kernel.release();
+
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public Cone getPosition() {
+        return cone;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetectionOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetectionOld.java
new file mode 100644
index 0000000..54c2f19
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/PoleDetectionOld.java
@@ -0,0 +1,103 @@
+package org.firstinspires.ftc.teamcode.old.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class PoleDetectionOld extends OpenCvPipeline {
+    private volatile boolean isPole = false;
+
+    // TOPLEFT anchor point for the bounding box
+    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(50, 85);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 50;
+    public static int REGION_HEIGHT = 30;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
+            upper_yellow_bounds = new Scalar(255, 255, 150, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255);
+
+    // Percent and mat definitions
+    private double yelPercent;
+    private final Mat yelMat = new Mat();
+    private Mat blurredMat = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+//
+//        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+//        Core.inRange(input, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+
+        // Gets color specific values
+//        Imgproc.Canny(yelMat, yelMat, 100, 200);
+        yelPercent = Core.countNonZero(yelMat);
+
+        // Calculates the highest amount of pixels being covered on each side
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (yelPercent >= 100) {
+            isPole = true;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        } else {
+            isPole = false;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        blurredMat.release();
+        yelMat.release();
+        kernel.release();
+
+        return input;
+    }
+
+    public boolean getPole() {
+        return isPole;
+    }
+
+    public double getPercent() {
+        return yelPercent;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNewOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNewOld.java
new file mode 100644
index 0000000..b70213c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionNewOld.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.old.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class SleeveDetectionNewOld extends OpenCvPipeline {
+
+    public enum ParkingPosition {
+        LEFT,
+        CENTER,
+        RIGHT
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(65, 50);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 25;
+    public static int REGION_HEIGHT = 15;
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255),
+            MAGENTA = new Scalar(255, 0, 255);
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile ParkingPosition position = ParkingPosition.LEFT;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Get the submat frame, and then sum all the values
+        Mat areaMat = input.submat(new Rect(sleeve_pointA, sleeve_pointB));
+        Scalar sumColors = Core.sumElems(areaMat);
+
+        double minColor = Math.min(sumColors.val[0], Math.min(sumColors.val[1], sumColors.val[2]));
+
+        if (sumColors.val[0] == minColor) {
+            position = ParkingPosition.CENTER;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        } else if (sumColors.val[1] == minColor) {
+            position = ParkingPosition.RIGHT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    MAGENTA,
+                    2
+            );
+        } else {
+            position = ParkingPosition.LEFT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        }
+
+        // Release and return input
+        areaMat.release();
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public ParkingPosition getPosition() {
+        return position;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRightOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRightOld.java
new file mode 100644
index 0000000..1237a57
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/pipeline/SleeveDetectionRightOld.java
@@ -0,0 +1,144 @@
+package org.firstinspires.ftc.teamcode.old.pipeline;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class SleeveDetectionRightOld extends OpenCvPipeline {
+    /*
+    YELLOW  = Parking Left
+    CYAN    = Parking Middle
+    MAGENTA = Parking Right
+     */
+
+    public enum ParkingPosition {
+        LEFT,
+        CENTER,
+        RIGHT
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(125, 175);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 25;
+    public static int REGION_HEIGHT = 15;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+//            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
+//            upper_yellow_bounds = new Scalar(255, 255, 150, 255),
+//            lower_cyan_bounds = new Scalar(0, 120, 120, 255),
+//            upper_cyan_bounds = new Scalar(150, 255, 255, 255),
+//            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
+//            upper_magenta_bounds = new Scalar(255, 170, 255, 255);
+
+            lower_yellow_bounds = new Scalar(120, 120, 0, 255),
+            upper_yellow_bounds = new Scalar(255, 255, 70, 255),
+            lower_cyan_bounds = new Scalar(2, 82, 176, 255),
+            upper_cyan_bounds = new Scalar(48, 255, 255, 255),
+            lower_magenta_bounds = new Scalar(125, 3, 176, 255),
+            upper_magenta_bounds = new Scalar(255, 43, 255, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255),
+            MAGENTA = new Scalar(255, 0, 255);
+
+    // Percent and mat definitions
+    private double yelPercent, cyaPercent, magPercent;
+    private final Mat yelMat = new Mat();
+    private final Mat cyaMat = new Mat();
+    private final Mat magMat = new Mat();
+    private Mat blurredMat = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile ParkingPosition position = ParkingPosition.LEFT;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+        Imgproc.cvtColor(input, blurredMat, Imgproc.COLOR_RGB2HSV);
+        blurredMat.convertTo(blurredMat, -1, 1, 10);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+        Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
+        Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
+
+        // Gets color specific values
+        yelPercent = Core.countNonZero(yelMat);
+        cyaPercent = Core.countNonZero(cyaMat);
+        magPercent = Core.countNonZero(magMat);
+
+        // Calculates the highest amount of pixels being covered on each side
+        double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
+
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (maxPercent == yelPercent) {
+            position = ParkingPosition.LEFT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        } else if (maxPercent == cyaPercent) {
+            position = ParkingPosition.CENTER;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        } else if (maxPercent == magPercent) {
+            position = ParkingPosition.RIGHT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    MAGENTA,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        blurredMat.release();
+        yelMat.release();
+        cyaMat.release();
+        magMat.release();
+        kernel.release();
+
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public ParkingPosition getPosition() {
+        return position;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
new file mode 100644
index 0000000..2b68e85
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
@@ -0,0 +1,145 @@
+package org.firstinspires.ftc.teamcode.old.subsystem;
+
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+
+public class LinearSlide extends Subsystem {
+    // Useful Constants
+    public static final double MIN_POWER = 0;
+    public static final double HOLD_POWER = 0.1;
+    public static final double MAX_POWER = 1;
+    public static final int MIN_HEIGHT = 0;
+    public static final int LOW_HEIGHT = 1600;
+    public static final int MEDIUM_HEIGHT = 2760;
+    public static final int HIGH_HEIGHT = 3870;
+    public static final int MAX_HEIGHT = 4000;
+    public static final int INCREMENT_STEPS = 20;
+    public static final double GRIP_MIN = 0;
+    public static final double GRIP_MAX = 0.6;
+
+    // Hardware
+    public DcMotorEx slideMotor;
+    public Servo leftGripper, rightGripper;
+
+    private boolean gripperClosed = false;
+    private boolean dPadPressed = false;
+    private int armMotorSteps = 0;
+
+    public LinearSlide(RobotOpMode opMode) {
+        super(opMode);
+    }
+
+    @Override
+    public void setup() {
+        // Initialize slide motor
+        slideMotor = opMode.hardwareMap.get(DcMotorEx.class, "motor_slide");
+        slideMotor.setDirection(DcMotorEx.Direction.REVERSE);
+        slideMotor.setTargetPosition(MIN_HEIGHT);
+        slideMotor.setPower(MAX_POWER);
+        slideMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
+        slideMotor.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
+        slideMotor.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.BRAKE);
+
+        // Initialize gripper servos
+        leftGripper = opMode.hardwareMap.get(Servo.class, "grip1");
+        rightGripper = opMode.hardwareMap.get(Servo.class, "grip2");
+    }
+
+    public void updateTelemetry() {
+        // Add some debug information
+        opMode.telemetry.addData("Target arm motor steps:", armMotorSteps);
+        opMode.telemetry.addData("Actual arm motor steps:", slideMotor.getCurrentPosition());
+        opMode.telemetry.addData("Arm Power:", slideMotor.getPower());
+        opMode.telemetry.addData("Arm Current (A):", slideMotor.getCurrent(CurrentUnit.AMPS));
+        opMode.telemetry.addData("Arm ZeroPower:", slideMotor.getZeroPowerBehavior());
+    }
+
+    public void readGamepad() {
+        // Set arm steps to predefined height with buttons
+        if (opMode.gamepad2.a) {
+            armMotorSteps = MIN_HEIGHT;
+        } else if (opMode.gamepad2.b) {
+            armMotorSteps = LOW_HEIGHT;
+        } else if (opMode.gamepad2.x) {
+            armMotorSteps = MEDIUM_HEIGHT;
+        } else if (opMode.gamepad2.y) {
+            armMotorSteps = HIGH_HEIGHT;
+        }
+
+        // Increase arm steps by DPAD increments
+        if (opMode.gamepad2.dpad_up) {
+            armMotorSteps += INCREMENT_STEPS;
+            dPadPressed = true;
+        } else if (opMode.gamepad2.dpad_down) {
+            armMotorSteps -= INCREMENT_STEPS;
+            dPadPressed = true;
+        } else if (dPadPressed) {
+            armMotorSteps = slideMotor.getCurrentPosition();
+            dPadPressed = false;
+        }
+
+        setSlide(armMotorSteps);
+
+        // If motor is busy, run motor at max power.
+        // If motor is not busy, hold at current position or stop at lowest height
+        if (slideMotor.isBusy()) {
+            slideMotor.setPower(MAX_POWER);
+        } else {
+            slideMotor.setPower(HOLD_POWER);
+            if (slideMotor.getCurrentPosition() == MIN_HEIGHT) {
+                slideMotor.setPower(MIN_POWER);
+            }
+        }
+
+        if (opMode.gamepad1.right_trigger > 0.5) {
+            gripperClosed = true;
+        } else if (opMode.gamepad1.left_trigger > 0.5) {
+            gripperClosed = false;
+        }
+
+        if (gripperClosed) {
+            grab();
+        } else {
+            release();
+        }
+    }
+
+    public void grab() {
+        leftGripper.setPosition(GRIP_MAX + 0.1);
+        rightGripper.setPosition(GRIP_MIN - 0.1);
+    }
+
+    public void grabAndWait() {
+        grab();
+        while (leftGripper.getPosition() < GRIP_MAX || rightGripper.getPosition() > GRIP_MIN) {
+            opMode.idle();
+        }
+        opMode.sleep(300);
+    }
+
+    public void release() {
+        leftGripper.setPosition(GRIP_MIN);
+        rightGripper.setPosition(GRIP_MAX);
+    }
+
+    public void releaseAndWait() {
+        release();
+        while (leftGripper.getPosition() > GRIP_MIN || rightGripper.getPosition() < GRIP_MAX) {
+            opMode.idle();
+        }
+    }
+
+    public void setSlide(int steps) {
+        armMotorSteps = Range.clip(steps, MIN_HEIGHT, MAX_HEIGHT);
+        slideMotor.setTargetPosition(armMotorSteps);
+    }
+
+    public void setSlideAndWait(int steps) {
+        setSlide(steps);
+        opMode.blockOn(slideMotor);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/SensorC.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/SensorC.java
new file mode 100644
index 0000000..a44b941
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/SensorC.java
@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.teamcode.old.subsystem;
+
+import android.graphics.Color;
+
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+
+
+public class SensorC extends Subsystem {
+
+    private NormalizedColorSensor colorSensor;
+    private NormalizedRGBA colors;
+    private float gain = 2;
+    final float[] hsvValues = new float[3];
+
+    public SensorC(RobotOpMode opMode) {
+        super(opMode);
+    }
+
+    @Override
+    public void setup() {
+        colorSensor = opMode.hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
+        colors = colorSensor.getNormalizedColors();
+        colorSensor.setGain(gain);
+    }
+
+    public void update() {
+        colors = colorSensor.getNormalizedColors();
+        Color.colorToHSV(colors.toColor(), hsvValues);
+    }
+
+    public float getHue() {
+        update();
+        return hsvValues[0];
+    }
+
+    public float getSaturation() {
+        update();
+        return hsvValues[1];
+    }
+
+    public float getValue() {
+        update();
+        return hsvValues[2];
+    }
+
+    public float getRed() {
+        update();
+        return colors.red;
+    }
+
+    public float getBlue() {
+        update();
+        return colors.blue;
+    }
+
+    public float getGreen() {
+        update();
+        return colors.green;
+    }
+
+    public float getAlpha() {
+        update();
+        return colors.alpha;
+    }
+
+    public float getGain() {
+        update();
+        return colorSensor.getGain();
+    }
+
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
new file mode 100644
index 0000000..e5d15fa
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/StrafeDrive.java
@@ -0,0 +1,358 @@
+package org.firstinspires.ftc.teamcode.old.subsystem;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+
+public class StrafeDrive extends Subsystem {
+
+    /* -------Constants------- */
+
+    //Static Constants(Don't Change)
+    static final double RAW_TICKS_PER_ROTATION = 28;
+    static final double MAX_MOTOR_RPM = 6000;
+    static final double GEAR_RATIO = 0.05291; //output divided by input
+    static final double WHEEL_DIAMETER = 3.0; //inches
+
+    //Calculated Constants
+    static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
+    static final double GEARED_TICKS_PER_ROTATION = RAW_TICKS_PER_ROTATION / GEAR_RATIO;
+    static final double GEARED_MOTOR_RPM = MAX_MOTOR_RPM * GEAR_RATIO;
+    static final double INCHES_PER_TICK = WHEEL_CIRCUMFERENCE / GEARED_TICKS_PER_ROTATION;
+    static final double MAX_SPEED = WHEEL_CIRCUMFERENCE * (GEARED_MOTOR_RPM / 60);
+
+    //Constants that you can Change
+    static final double HIGH_POWER = 0.85;
+    static final double LOW_POWER = 0.35;
+    static final double DEAD_ZONE_P1 = 0.05;
+    static final double DEAD_ZONE_P2 = 0.05;
+
+    /* -------Variables------- */
+
+    //Hardware
+    public IMU imu;
+    public DcMotor frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor;
+
+
+    //Misc Variables
+    public double botHeading;
+    private double limiter = HIGH_POWER;
+    private boolean field = false;
+
+    /* -------Constructor------- */
+
+    //Default Constructor
+    public StrafeDrive(RobotOpMode opMode) {
+        super(opMode);
+    }
+
+    @Override
+    /* -------Initialization------- */
+
+    //Initializes drive
+    public void setup() {
+        frontLeftMotor = opMode.hardwareMap.get(DcMotor.class, "motor_front_left");
+        frontRightMotor = opMode.hardwareMap.get(DcMotor.class, "motor_front_right");
+        backLeftMotor = opMode.hardwareMap.get(DcMotor.class, "motor_back_left");
+        backRightMotor = opMode.hardwareMap.get(DcMotor.class, "motor_back_right");
+        imu = opMode.hardwareMap.get(IMU.class, "imu");
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
+        imu.initialize(parameters);
+        botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+        backLeftMotor.setDirection(DcMotor.Direction.REVERSE);
+        frontLeftMotor.setDirection(DcMotor.Direction.REVERSE);
+        backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    }
+
+    /* -------Telemetry------- */
+
+    //Updates telemetry, sends data to driver station
+    public void updateTelemetry() {
+        opMode.telemetry.addData("Limiter: ", limiter);
+        opMode.telemetry.addData("Heading: ", botHeading);
+        opMode.telemetry.addData("Field Centric?: ", field);
+        opMode.telemetry.addData("Back Left Power: ", backLeftMotor.getPower());
+        opMode.telemetry.addData("Back Right Power: ", backRightMotor.getPower());
+        opMode.telemetry.addData("Front Left Power: ", frontLeftMotor.getPower());
+        opMode.telemetry.addData("Front Right Power: ", frontRightMotor.getPower());
+    }
+
+
+    /* -------Conversions------- */
+
+    //Converts inches to motor rotation ticks
+    public static int toTicks(double inches) {
+        return (int) Math.round(inches / INCHES_PER_TICK);
+    }
+
+    //Converts distance to time based on the velocity of the robot
+    public static double distToTime(double inches, double power) {
+        return Math.round((inches / (MAX_SPEED * power)) * 100) / 100.0;
+    }
+
+    /* -------Update Heading------- */
+
+    //Updates heading in degrees
+    public void updateHeadingDeg() {
+        botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+    }
+
+    //Updates heading in radians
+    public void updateHeadingRad() {
+        botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    /* -------Bulk Motor State Functions------- */
+
+    //Set all motor modes
+    public void setModes(DcMotor.RunMode mode) {
+        backLeftMotor.setMode(mode);
+        backRightMotor.setMode(mode);
+        frontLeftMotor.setMode(mode);
+        frontRightMotor.setMode(mode);
+    }
+
+    //Set all motor powers
+    public void setPowers(float speed) {
+        backLeftMotor.setPower(speed);
+        backRightMotor.setPower(speed);
+        frontLeftMotor.setPower(speed);
+        frontRightMotor.setPower(speed);
+    }
+
+    //Set motor targets to their variables
+    public void setMotorTargets(double blPos, double brPos, double flPos, double frPos) {
+        backLeftMotor.setTargetPosition((int) blPos);
+        backRightMotor.setTargetPosition((int) brPos);
+        frontLeftMotor.setTargetPosition((int) flPos);
+        frontRightMotor.setTargetPosition((int) frPos);
+    }
+
+    //Sets all motor powers for complex movement
+    public void driveBot(double forward, double strafe, double rotate) {
+        frontLeftMotor.setPower((forward + strafe + rotate));
+        backLeftMotor.setPower((forward - strafe + rotate));
+        frontRightMotor.setPower((forward + strafe - rotate));
+        backRightMotor.setPower((forward - strafe - rotate));
+    }
+
+    /* -------Autonomous Encoder Based Drive------- */
+
+    //Move forward/backward, in ticks
+    public void drive(int leftMove, int rightMove, float speed) {
+
+        double blPos = backLeftMotor.getCurrentPosition() + leftMove;
+        double brPos = backRightMotor.getCurrentPosition() + rightMove;
+        double flPos = frontLeftMotor.getCurrentPosition() + leftMove;
+        double frPos = frontRightMotor.getCurrentPosition() + rightMove;
+
+        setMotorTargets(blPos, brPos, flPos, frPos);
+
+        setModes(DcMotor.RunMode.RUN_TO_POSITION);
+
+        setPowers(speed);
+        opMode.blockOn(backLeftMotor, backRightMotor, frontLeftMotor, frontRightMotor);
+    }
+
+    //Strafe left/right, in ticks
+    public void strafe(int move, float speed) {
+
+        double blPos = backLeftMotor.getCurrentPosition() - move;
+        double brPos = backRightMotor.getCurrentPosition() - move;
+        double flPos = frontLeftMotor.getCurrentPosition() + move;
+        double frPos = frontRightMotor.getCurrentPosition() + move;
+
+        setMotorTargets(blPos, brPos, flPos, frPos);
+
+        setModes(DcMotor.RunMode.RUN_TO_POSITION);
+
+        setPowers(speed);
+        opMode.blockOn(backLeftMotor, backRightMotor, frontLeftMotor, frontRightMotor);
+    }
+
+    //Drives using inches instead of ticks
+    public void driveInches(double leftInches, double rightInches, float speed) {
+        drive(toTicks(leftInches), toTicks(rightInches), speed);
+    }
+
+    //Strafes using inches instead of ticks
+    public void strafeInches(double inches, float speed) {
+        strafe(toTicks(inches), speed);
+    }
+
+    /* -------Autonomous Gyro/Time Based Drive------- */
+
+    //Turn using Gyro, heading is absolute
+    public void turnToAbs(double target, double power) {
+        updateHeadingDeg();
+        setModes(DcMotor.RunMode.RUN_USING_ENCODER);
+        while ((botHeading < target - 2 || botHeading > target + 2) && opMode.opModeIsActive()) {
+            driveBot(0, 0, power);
+
+            updateHeadingDeg();
+            opMode.telemetry.addData("Heading: ", botHeading);
+            opMode.telemetry.update();
+        }
+        setPowers(0);
+    }
+
+    //Turn using Gyro, heading is based on robot's initial facing
+    public void turnToRel(double relTarg, double power) {
+        updateHeadingDeg();
+        double target = botHeading + relTarg;
+        if (target > 180) {
+            target -= 360;
+        }
+        if (target < -180) {
+            target += 360;
+        }
+        turnToAbs(target, power);
+    }
+
+    //Complex time based gyro movement- movement is absolute
+    public void rotateAndMove(double seconds, double rotateTarget, double forwardPower, double strafePower, double rotatePower) {
+        ElapsedTime timer = new ElapsedTime();
+        timer.reset();
+        double fieldForward;
+        double fieldStrafe;
+        updateHeadingDeg();
+        setModes(DcMotor.RunMode.RUN_USING_ENCODER);
+        while (timer.time() < seconds && (botHeading < rotateTarget - 2 || botHeading > rotateTarget + 2) && opMode.opModeIsActive()) {
+            updateHeadingRad();
+            fieldForward = strafePower * Math.sin(-botHeading) + forwardPower * Math.cos(-botHeading);
+            fieldStrafe = strafePower * Math.cos(-botHeading) - forwardPower * Math.sin(-botHeading);
+            updateHeadingDeg();
+
+            driveBot(fieldForward, fieldStrafe, rotatePower);
+            opMode.telemetry.addData("Heading: ", botHeading);
+            opMode.telemetry.addData("Time: ", timer.time());
+            opMode.telemetry.update();
+
+        }
+        while (timer.time() < seconds && opMode.opModeIsActive()) {
+
+            updateHeadingRad();
+            fieldForward = strafePower * Math.sin(-botHeading) + forwardPower * Math.cos(-botHeading);
+            fieldStrafe = strafePower * Math.cos(-botHeading) - forwardPower * Math.sin(-botHeading);
+            driveBot(fieldForward, fieldStrafe, 0);
+            opMode.telemetry.addData("Heading: ", botHeading);
+            opMode.telemetry.addData("Time: ", timer.time());
+            opMode.telemetry.update();
+        }
+        setPowers(0);
+    }
+
+    //Complex gyro based movement in terms of distance instead of time- Also has added functionality for easy diagonal movement (AT LEAST ONE OF forwardDist OR strafeDist MUST BE != 0)
+    public void rotateAndMoveInches(double rotateTarget, double forwardDist, double strafeDist, double maxMovePower, double rotatePower) {
+        double maxDist = Math.max(Math.abs(forwardDist), Math.abs(strafeDist));
+        double forwardPower;
+        double strafePower;
+        double travelTime;
+        if (maxDist == Math.abs(forwardDist)) {
+            forwardPower = Math.abs(maxMovePower);
+            if (forwardDist < 0) {
+                forwardPower *= -1;
+            }
+            strafePower = forwardPower * (strafeDist / forwardDist);
+        } else {
+            strafePower = Math.abs(maxMovePower);
+            if (strafeDist < 0) {
+                strafePower *= -1;
+            }
+            forwardPower = strafePower * (forwardDist / strafeDist);
+        }
+        travelTime = distToTime(maxDist, maxMovePower);
+        rotateAndMove(travelTime, rotateTarget, forwardPower, strafePower, rotatePower);
+    }
+
+    /* -------TeleOp Functions------- */
+
+    // 2 Player TeleOp
+    public void updateByTwoGamepads() {
+        double forward;
+        double strafe;
+        double rotate;
+        double fieldForward;
+        double fieldStrafe;
+
+        if (opMode.gamepad1.a) {
+            limiter = HIGH_POWER;
+        } else if (opMode.gamepad1.b) {
+            limiter = LOW_POWER;
+        }
+        if (opMode.gamepad1.x) {
+            imu.resetYaw();
+            field = true;
+        } else if (opMode.gamepad1.y) {
+            field = false;
+        }
+
+        forward = -opMode.gamepad1.left_stick_y * limiter;
+        strafe = opMode.gamepad1.left_stick_x * limiter;
+        rotate = opMode.gamepad1.right_stick_x * limiter;
+
+        if (Math.abs(-opMode.gamepad1.left_stick_y) < DEAD_ZONE_P1) {
+            forward = -opMode.gamepad2.left_stick_y * LOW_POWER;
+            if (Math.abs(opMode.gamepad2.left_stick_y) < DEAD_ZONE_P2) {
+                forward = 0;
+            }
+        }
+        if (Math.abs(opMode.gamepad1.left_stick_x) < DEAD_ZONE_P1) {
+            strafe = opMode.gamepad2.left_stick_x * LOW_POWER;
+            if (Math.abs(opMode.gamepad2.left_stick_x) < DEAD_ZONE_P2) {
+                strafe = 0;
+            }
+        }
+        if (Math.abs(opMode.gamepad1.right_stick_x) < DEAD_ZONE_P1) {
+            rotate = opMode.gamepad2.right_stick_x * LOW_POWER;
+            if (Math.abs(opMode.gamepad2.right_stick_x) < DEAD_ZONE_P2) {
+                rotate = 0;
+            }
+        }
+        fieldForward = strafe * Math.sin(-botHeading) + forward * Math.cos(-botHeading);
+        fieldStrafe = strafe * Math.cos(-botHeading) - forward * Math.sin(-botHeading);
+
+        updateHeadingRad();
+        if (!field) {
+            driveBot(forward, strafe, rotate);
+        } else {
+            driveBot(fieldForward, fieldStrafe, rotate);
+        }
+
+
+    }
+
+    // 1 Player TeleOp
+    public void updateByGamepad() {
+        double forward;
+        double strafe;
+        double rotate;
+
+        forward = -opMode.gamepad1.left_stick_y * limiter;
+        strafe = opMode.gamepad1.left_stick_x * limiter;
+        rotate = opMode.gamepad1.right_stick_x * limiter;
+        if (Math.abs(-opMode.gamepad1.left_stick_y) < 0.01) {
+            forward = 0;
+        }
+        if (Math.abs(opMode.gamepad1.left_stick_x) < 0.01) {
+            strafe = 0;
+        }
+        if (Math.abs(opMode.gamepad1.right_stick_x) < 0.01) {
+            rotate = 0;
+        }
+
+        driveBot(forward, strafe, rotate);
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
new file mode 100644
index 0000000..20ae19f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Subsystem.java
@@ -0,0 +1,13 @@
+package org.firstinspires.ftc.teamcode.old.subsystem;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+
+abstract public class Subsystem {
+    protected final RobotOpMode opMode;
+
+    public Subsystem(RobotOpMode opMode) {
+        this.opMode = opMode;
+    }
+
+    abstract public void setup();
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
new file mode 100644
index 0000000..81096e9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/Webcam.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.old.subsystem;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class Webcam extends Subsystem {
+    public OpenCvCamera camera;
+    public final OpenCvPipeline pipeline;
+    public final String name;
+    public final static int WIDTH = 320;
+    public final static int HEIGHT = 240;
+
+    public Webcam(RobotOpMode opMode, String name, OpenCvPipeline pipeline) {
+        super(opMode);
+        this.name = name;
+        this.pipeline = pipeline;
+    }
+
+    @Override
+    public void setup() {
+        int monitorViewId = opMode.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", opMode.hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(opMode.hardwareMap.get(WebcamName.class, name), monitorViewId);
+        camera.setPipeline(pipeline);
+
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            @Override
+            public void onOpened() {
+                camera.startStreaming(WIDTH, HEIGHT, OpenCvCameraRotation.SIDEWAYS_LEFT);
+            }
+
+            @Override
+            public void onError(int errorCode) {
+            }
+        });
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
new file mode 100644
index 0000000..9e86740
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/SensorColor.java
@@ -0,0 +1,222 @@
+/* Copyright (c) 2017-2020 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.old.teleop;
+
+import android.app.Activity;
+import android.graphics.Color;
+import android.view.View;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/**
+ * This is an example LinearOpMode that shows how to use a color sensor in a generic
+ * way, regardless of which particular make or model of color sensor is used. The Op Mode
+ * assumes that the color sensor is configured with a name of "sensor_color".
+ *
+ * There will be some variation in the values measured depending on the specific sensor you are using.
+ *
+ * You can increase the gain (a multiplier to make the sensor report higher values) by holding down
+ * the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad.
+ *
+ * If the color sensor has a light which is controllable from software, you can use the X button on
+ * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have
+ * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2.
+ *
+ * If the color sensor also supports short-range distance measurements (usually via an infrared
+ * proximity sensor), the reported distance will be written to telemetry. As of September 2020,
+ * the only color sensors that support this are the ones from REV Robotics. These infrared proximity
+ * sensor measurements are only useful at very small distances, and are sensitive to ambient light
+ * and surface reflectivity. You should use a different sensor if you need precise distance measurements.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this Op Mode to the Driver Station OpMode list
+ */
+@TeleOp(name = "Sensor: Color", group = "Sensor")
+public class SensorColor extends LinearOpMode {
+
+  /** The colorSensor field will contain a reference to our color sensor hardware object */
+  NormalizedColorSensor colorSensor;
+
+  /** The relativeLayout field is used to aid in providing interesting visual feedback
+   * in this sample application; you probably *don't* need this when you use a color sensor on your
+   * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */
+  View relativeLayout;
+
+  /**
+   * The runOpMode() method is the root of this Op Mode, as it is in all LinearOpModes.
+   * Our implementation here, though is a bit unusual: we've decided to put all the actual work
+   * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is
+   * that in this sample we're changing the background color of the robot controller screen as the
+   * Op Mode runs, and we want to be able to *guarantee* that we restore it to something reasonable
+   * and palatable when the Op Mode ends. The simplest way to do that is to use a try...finally
+   * block around the main, core logic, and an easy way to make that all clear was to separate
+   * the former from the latter in separate methods.
+   */
+  @Override public void runOpMode() {
+
+    // Get a reference to the RelativeLayout so we can later change the background
+    // color of the Robot Controller app to match the hue detected by the RGB sensor.
+    int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
+    relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);
+
+    try {
+      runSample(); // actually execute the sample
+    } finally {
+      // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off
+      // as pure white, but it's too much work to dig out what actually was used, and this is good
+      // enough to at least make the screen reasonable again.
+      // Set the panel back to the default color
+      relativeLayout.post(new Runnable() {
+        public void run() {
+          relativeLayout.setBackgroundColor(Color.WHITE);
+        }
+      });
+      }
+  }
+
+  protected void runSample() {
+    // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the
+    // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3)
+    // can give very low values (depending on the lighting conditions), which only use a small part
+    // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions,
+    // you should use a smaller gain than in dark conditions. If your gain is too high, all of the
+    // colors will report at or near 1, and you won't be able to determine what color you are
+    // actually looking at. For this reason, it's better to err on the side of a lower gain
+    // (but always greater than  or equal to 1).
+    float gain = 2;
+
+    // Once per loop, we will update this hsvValues array. The first element (0) will contain the
+    // hue, the second element (1) will contain the saturation, and the third element (2) will
+    // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
+    // for an explanation of HSV color.
+    final float[] hsvValues = new float[3];
+
+    // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current
+    // state of the X button on the gamepad
+    boolean xButtonPreviouslyPressed = false;
+    boolean xButtonCurrentlyPressed = false;
+
+    // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over
+    // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while
+    // the values you get from ColorSensor are dependent on the specific sensor you're using.
+    colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
+
+    // If possible, turn the light on in the beginning (it might already be on anyway,
+    // we just make sure it is if we can).
+    if (colorSensor instanceof SwitchableLight) {
+      ((SwitchableLight)colorSensor).enableLight(true);
+    }
+
+    // Wait for the start button to be pressed.
+    waitForStart();
+
+    // Loop until we are asked to stop
+    while (opModeIsActive()) {
+      // Explain basic gain information via telemetry
+      telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n");
+      telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n");
+
+      // Update the gain value if either of the A or B gamepad buttons is being held
+      if (gamepad1.a) {
+        // Only increase the gain by a small amount, since this loop will occur multiple times per second.
+        gain += 0.005;
+      } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful.
+        gain -= 0.005;
+      }
+
+      // Show the gain value via telemetry
+      telemetry.addData("Gain", gain);
+
+      // Tell the sensor our desired gain value (normally you would do this during initialization,
+      // not during the loop)
+      colorSensor.setGain(gain);
+
+      // Check the status of the X button on the gamepad
+      xButtonCurrentlyPressed = gamepad1.x;
+
+      // If the button state is different than what it was, then act
+      if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) {
+        // If the button is (now) down, then toggle the light
+        if (xButtonCurrentlyPressed) {
+          if (colorSensor instanceof SwitchableLight) {
+            SwitchableLight light = (SwitchableLight)colorSensor;
+            light.enableLight(!light.isLightOn());
+          }
+        }
+      }
+      xButtonPreviouslyPressed = xButtonCurrentlyPressed;
+
+      // Get the normalized colors from the sensor
+      NormalizedRGBA colors = colorSensor.getNormalizedColors();
+
+      /* Use telemetry to display feedback on the driver station. We show the red, green, and blue
+       * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent
+       * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
+       * for an explanation of HSV color. */
+
+      // Update the hsvValues array by passing it to Color.colorToHSV()
+      Color.colorToHSV(colors.toColor(), hsvValues);
+
+      telemetry.addLine()
+              .addData("Red", "%.3f", colors.red)
+              .addData("Green", "%.3f", colors.green)
+              .addData("Blue", "%.3f", colors.blue);
+      telemetry.addLine()
+              .addData("Hue", "%.3f", hsvValues[0])
+              .addData("Saturation", "%.3f", hsvValues[1])
+              .addData("Value", "%.3f", hsvValues[2]);
+      telemetry.addData("Alpha", "%.3f", colors.alpha);
+
+      /* If this color sensor also has a distance sensor, display the measured distance.
+       * Note that the reported distance is only useful at very close range, and is impacted by
+       * ambient light and surface reflectivity. */
+      if (colorSensor instanceof DistanceSensor) {
+        telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM));
+      }
+
+      telemetry.update();
+
+      // Change the Robot Controller's background color to match the color detected by the color sensor.
+      relativeLayout.post(new Runnable() {
+        public void run() {
+          relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues));
+        }
+      });
+    }
+  }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java
new file mode 100644
index 0000000..bab2bf7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.old.teleop;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.old.RobotOpMode;
+import org.firstinspires.ftc.teamcode.old.subsystem.LinearSlide;
+import org.firstinspires.ftc.teamcode.old.subsystem.StrafeDrive;
+
+@TeleOp(name = "Two Player TeleOp Old")
+public class TwoPlayerTeleopOld extends RobotOpMode {
+    private final StrafeDrive drive = new StrafeDrive(this);
+    private final LinearSlide slide = new LinearSlide(this);
+
+    @Override
+    public void runOpMode() {
+        addSubsystems(drive, slide);
+        waitForStart();
+
+        while (opModeIsActive()) {
+            drive.updateByTwoGamepads();
+            drive.updateTelemetry();
+            slide.readGamepad();
+            slide.updateTelemetry();
+            telemetry.update();
+        }
+    }
+}

From 8df0a411bdf207868266c4102013f7711e85b83b Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 3 Mar 2023 18:48:31 -0500
Subject: [PATCH 35/43] unholy merge of old and new code

---
 .../ftc/teamcode/old/older/Auto/BOEcode.java  | 114 +++++++++++
 .../old/older/Auto/BetterRobotAutonomous.java |  91 +++++++++
 .../old/older/Auto/NewBotAutoLeft.java        | 137 +++++++++++++
 .../old/older/Auto/NewBotAutoRight.java       | 147 ++++++++++++++
 .../old/older/Auto/OldRobotAutonomous.java    |  49 +++++
 .../older/Auto/OldRobotAutonomousOther.java   |  50 +++++
 .../old/older/Auto/StrafeBotAutoLeft.java     |  55 ++++++
 .../teamcode/old/older/Robots/NewRobot.java   |  84 ++++++++
 .../teamcode/old/older/Robots/OldRobot.java   |  83 ++++++++
 .../old/older/Subsystems/BackTankDrive.java   |  79 ++++++++
 .../old/older/Subsystems/BoeDetection.java    | 185 ++++++++++++++++++
 .../old/older/Subsystems/FullDetection.java   | 170 ++++++++++++++++
 .../old/older/Subsystems/LinearSlide.java     | 102 ++++++++++
 .../older/Subsystems/NewPoleDetection.java    |  84 ++++++++
 .../old/older/Subsystems/PoleDetection.java   | 100 ++++++++++
 .../Subsystems/SingleJointGripperArm.java     | 102 ++++++++++
 .../old/older/Subsystems/SleeveDetection.java | 134 +++++++++++++
 .../old/older/Subsystems/StrafeDrive.java     | 176 +++++++++++++++++
 .../old/older/TeleOp/CameraRobotTeleOp.java   |  80 ++++++++
 .../old/older/TeleOp/NewRobotTeleOp.java      |  26 +++
 .../old/older/TeleOp/OldRobotTeleOp.java      |  22 +++
 .../ftc/teamcode/old/older/Util/Maths.java    |  30 +++
 .../ftc/teamcode/old/older/Util/Measure.java  |  12 ++
 23 files changed, 2112 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java
new file mode 100644
index 0000000..dc18033
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java
@@ -0,0 +1,114 @@
+package org.firstinspires.ftc.teamcode.old.Auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
+import org.firstinspires.ftc.teamcode.old.Subsystems.BoeDetection;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+@Disabled
+public class BOEcode extends LinearOpMode {
+
+    BoeDetection coneDetection = new BoeDetection();
+    OpenCvCamera camera;
+    String webcamName = "Webcam 1";
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
+        coneDetection = new BoeDetection();
+        camera.setPipeline(coneDetection);
+
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            @Override
+            public void onOpened() {
+                camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT);
+            }
+
+            @Override
+            public void onError(int errorCode) {
+            }
+        });
+
+        while (!isStarted()) {
+            telemetry.addData("Cone: ", coneDetection.getPosition());
+            telemetry.update();
+        }
+
+
+        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
+
+        //Put motors in encoder mode
+        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.singleJointGripperArm.grab();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+
+//        robot.singleJointGripperArm.ungrab();
+//        robot.backTankDrive.drive(-400, -400, 0.5f);
+//        robot.backTankDrive.drive(-520, 520, 0.5f);
+//        robot.singleJointGripperArm.setArm(290);
+//        robot.backTankDrive.drive(-200, -200, 0.25f);
+//        robot.singleJointGripperArm.grab();
+//        robot.backTankDrive.drive(520, 520, 0.25f);
+//        robot.singleJointGripperArm.setArm(0);
+//        robot.backTankDrive.drive(375, -375, 0.5f);
+        int straight = 0;
+        boolean turningRight = true;
+        while (coneDetection.getPosition() == BoeDetection.Cone.UNDECIDED || coneDetection.getPosition() == BoeDetection.Cone.RED) {
+
+            if (turningRight) {
+                robot.backTankDrive.drive(-10, 10, 0.5f);
+                straight += 10;
+            } else {
+                robot.backTankDrive.drive(10, -10, 0.5f);
+                straight -= 10;
+            }
+            if (straight >= 500) {
+                turningRight = false;
+            } else if (straight <= -500) {
+                turningRight = true;
+            }
+        }
+
+        robot.backTankDrive.backLeftMotor.setPower(0);
+        robot.backTankDrive.backRightMotor.setPower(0);
+
+        switch (coneDetection.getPosition()) {
+            case RED:
+                robot.singleJointGripperArm.grab();
+                robot.backTankDrive.drive(-3120, -3120, 0.5f);
+                robot.singleJointGripperArm.ungrab();
+                robot.backTankDrive.drive(straight, -straight, 0.5f);
+                robot.backTankDrive.drive(-1000, 1000, 0.25f);
+                robot.backTankDrive.drive(-3000, -3000, 0.5f);
+                robot.singleJointGripperArm.grab();
+                robot.backTankDrive.drive(1000, 1000, 0.5f);
+                break;
+            case BLUE:
+                robot.singleJointGripperArm.grab();
+                robot.backTankDrive.drive(-3120, -3120, 0.5f);
+                robot.singleJointGripperArm.ungrab();
+                robot.backTankDrive.drive(straight, -straight, 0.5f);
+                robot.backTankDrive.drive(1000, -1000, 0.25f);
+                robot.backTankDrive.drive(-3000, -3000, 0.5f);
+                robot.singleJointGripperArm.grab();
+                robot.backTankDrive.drive(1000, 1000, 0.5f);
+                break;
+
+            default:
+
+                break;
+        }
+
+    }
+
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java
new file mode 100644
index 0000000..310d4d3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java
@@ -0,0 +1,91 @@
+package org.firstinspires.ftc.teamcode.old.Auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.old.Util.Maths;
+
+@Disabled
+public class BetterRobotAutonomous extends LinearOpMode {
+    private DcMotor left;
+    private DcMotor right;
+    private DcMotor arm;
+    private Servo grip1;
+    private Servo grip2;
+    private int lPos;
+    private int rPos;
+    private double gripMin = 0.45;
+    private double gripMax = 1.0;
+
+    @Override
+    public void runOpMode() {
+        left = hardwareMap.get(DcMotor.class, "b_left");
+        right = hardwareMap.get(DcMotor.class, "b_right");
+        arm = hardwareMap.get(DcMotor.class, "arm");
+        grip1 = hardwareMap.get(Servo.class, "grip1");
+        grip2 = hardwareMap.get(Servo.class, "grip2");
+
+        //Put motors in encoder mode
+        left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+
+        right.setDirection(DcMotorSimple.Direction.REVERSE);
+        //You need to have wait for start or else bad things happen
+        ungrab();
+        waitForStart();
+
+        grab();
+        move(2080, 2080, 0.50);
+        armSet(89);
+        move(520, -520, 0.50);
+        ungrab();
+        move(-520, 520, 0.50);
+        armSet(0);
+        move(2080, 2080, 0.50);
+        move(-520, 520, 0.50);
+        move(520, -520, 0.50);
+
+
+
+
+    }
+    private void move(int leftTarget, int rightTarget, double speed) {
+        lPos += leftTarget;
+        rPos += rightTarget;
+
+        left.setTargetPosition(lPos);
+        right.setTargetPosition(rPos);
+
+        left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+        left.setPower(speed);
+        right.setPower(speed);
+
+        while(opModeIsActive() && left.isBusy() && right.isBusy()) {
+            idle();
+        }
+    }
+
+    private void armSet(int steps) {
+        arm.setTargetPosition(Maths.clamp(steps, 0, 450));
+        while (opModeIsActive() && arm.isBusy()) {
+            idle();
+        }
+    }
+    public void grab() {
+        grip1.setPosition(gripMax);
+        grip2.setPosition(1-gripMax);
+    }
+
+    public void ungrab() {
+        grip1.setPosition(gripMin);
+        grip2.setPosition(1-gripMin);
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java
new file mode 100644
index 0000000..ab176eb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java
@@ -0,0 +1,137 @@
+package org.firstinspires.ftc.teamcode.old.Auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
+import org.firstinspires.ftc.teamcode.old.Subsystems.SleeveDetection;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+@Disabled
+public class NewBotAutoLeft extends LinearOpMode {
+
+    SleeveDetection sleeveDetection = new SleeveDetection();
+    OpenCvCamera camera;
+    String webcamName = "Webcam 1";
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
+        sleeveDetection = new SleeveDetection();
+        camera.setPipeline(sleeveDetection);
+
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
+        {
+            @Override
+            public void onOpened()
+            {
+                camera.startStreaming(320,240, OpenCvCameraRotation.SIDEWAYS_LEFT);
+            }
+
+            @Override
+            public void onError(int errorCode) {}
+        });
+
+        while (!isStarted()) {
+            telemetry.addData("ROTATION: ", sleeveDetection.getPosition());
+            telemetry.update();
+        }
+
+
+        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
+
+        //Put motors in encoder mode
+        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.singleJointGripperArm.grab();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+
+//        robot.singleJointGripperArm.ungrab();
+//        robot.backTankDrive.drive(-400, -400, 0.5f);
+//        robot.backTankDrive.drive(-520, 520, 0.5f);
+//        robot.singleJointGripperArm.setArm(290);
+//        robot.backTankDrive.drive(-200, -200, 0.25f);
+//        robot.singleJointGripperArm.grab();
+//        robot.backTankDrive.drive(520, 520, 0.25f);
+//        robot.singleJointGripperArm.setArm(0);
+//        robot.backTankDrive.drive(375, -375, 0.5f);
+
+
+        switch(sleeveDetection.getPosition()) {
+
+            case LEFT:
+                placeMiddle(robot);
+//                robot.backTankDrive.drive(3000, 3000, 0.5f);
+//                robot.backTankDrive.drive(850, -850, 0.5f);
+//                robot.singleJointGripperArm.setArm(89);
+//                robot.singleJointGripperArm.ungrab();
+//                robot.backTankDrive.drive(-3300, -3300, 0.5f);
+//                robot.backTankDrive.drive(-450, 450, 0.25f);
+                robot.backTankDrive.drive(-2200, -2200, 0.75f);
+                break;
+            case RIGHT:
+                placeMiddle(robot);
+//                robot.backTankDrive.drive(150, 150, 0.5f);
+//                robot.backTankDrive.drive(-900, 900, 0.5f);
+//                robot.backTankDrive.drive(-2300, -2300, 0.5f);
+//                robot.backTankDrive.drive(900, -900, 0.5f);
+                robot.backTankDrive.drive(2450, 2450, 0.5f);
+                break;
+            default:
+                placeMiddle(robot);
+                break;
+        }
+
+    }
+    public void placeMiddle(OldRobot robot){
+        //First Cone
+        robot.singleJointGripperArm.ungrab();
+        robot.backTankDrive.drive(-3120, -3120, 1f);
+        robot.singleJointGripperArm.setArm(450);
+        robot.backTankDrive.drive(-500, 500, 0.6f);
+        robot.backTankDrive.drive(-350, -350, 0.25f);
+        robot.singleJointGripperArm.grab();
+        robot.backTankDrive.drive(500, 500, 0.25f);
+        robot.backTankDrive.drive(500, -500, 0.6f);
+        robot.singleJointGripperArm.setArm(0);
+
+        //Second Cone
+        robot.backTankDrive.drive(-2500, -2500, 1f);
+        robot.backTankDrive.drive(450, 450, 1f);
+        robot.singleJointGripperArm.setArm(125);
+        robot.backTankDrive.drive(1100, -1100, 0.5f);
+        robot.backTankDrive.drive(-1175, -1175, 0.5f);
+
+        robot.singleJointGripperArm.ungrab();
+        robot.singleJointGripperArm.setArm(300);
+        robot.backTankDrive.drive(200, -200, 0.6f);
+        robot.backTankDrive.drive(2000,2000, 0.5f);
+        robot.backTankDrive.drive(375, -375, 0.5f);
+        robot.backTankDrive.drive(-200,-200, 0.5f);
+        robot.singleJointGripperArm.setArm(125);
+        robot.singleJointGripperArm.grab();
+        robot.backTankDrive.drive(200,200, 0.5f);
+        robot.backTankDrive.drive(-375, 375, 0.5f);
+        robot.singleJointGripperArm.setArm(100);
+
+        //Third Cone
+        robot.backTankDrive.drive(-2000,-2000, 0.5f);
+        robot.singleJointGripperArm.ungrab();
+        robot.singleJointGripperArm.setArm(250);
+        robot.backTankDrive.drive(-1175, -1175, 0.5f);
+        robot.backTankDrive.drive(-375, 375, 0.5f);
+        robot.singleJointGripperArm.setArm(0);
+        robot.singleJointGripperArm.ungrab();
+        robot.singleJointGripperArm.setArm(100);
+        robot.backTankDrive.drive(375, -375, 0.5f);
+    }
+
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java
new file mode 100644
index 0000000..b4e99d8
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java
@@ -0,0 +1,147 @@
+package org.firstinspires.ftc.teamcode.old.Auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
+import org.firstinspires.ftc.teamcode.old.Subsystems.SleeveDetection;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+@Disabled
+public class NewBotAutoRight extends LinearOpMode {
+
+    SleeveDetection sleeveDetection = new SleeveDetection();
+    OpenCvCamera camera;
+    String webcamName = "Webcam 1";
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
+        sleeveDetection = new SleeveDetection();
+        camera.setPipeline(sleeveDetection);
+
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
+        {
+            @Override
+            public void onOpened()
+            {
+                camera.startStreaming(320,240, OpenCvCameraRotation.SIDEWAYS_LEFT);
+            }
+
+            @Override
+            public void onError(int errorCode) {}
+        });
+
+        while (!isStarted()) {
+            telemetry.addData("ROTATION: ", sleeveDetection.getPosition());
+            telemetry.update();
+        }
+
+
+        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
+
+        //Put motors in encoder mode
+        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.singleJointGripperArm.grab();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+
+//        robot.singleJointGripperArm.ungrab();
+//        robot.backTankDrive.drive(-400, -400, 0.5f);
+//        robot.backTankDrive.drive(-520, 520, 0.5f);
+//        robot.singleJointGripperArm.setArm(290);
+//        robot.backTankDrive.drive(-200, -200, 0.25f);
+//        robot.singleJointGripperArm.grab();
+//        robot.backTankDrive.drive(520, 520, 0.25f);
+//        robot.singleJointGripperArm.setArm(0);
+//        robot.backTankDrive.drive(375, -375, 0.5f);
+        switch(sleeveDetection.getPosition()) {
+            case LEFT:
+                placeMiddle(robot);
+//                robot.backTankDrive.drive(150, 150, 0.5f);
+//                robot.backTankDrive.drive(900, -900, 0.5f);
+//                robot.backTankDrive.drive(-2300, -2300, \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\0.5f);
+//                robot.backTankDrive.drive(-900, 900, 0.5f);
+                robot.backTankDrive.drive(2450, 2450, 0.5f);
+
+                break;
+            case RIGHT:
+                placeMiddle(robot);
+//                robot.backTankDrive.drive(3000, 3000, 0.5f);
+//                robot.backTankDrive.drive(-850, 850, 0.5f);
+//                robot.singleJointGripperArm.setArm(89);
+//                robot.singleJointGripperArm.ungrab();
+//                robot.backTankDrive.drive(-3300, -3300, 0.5f);
+//                robot.backTankDrive.drive(450, -450, 0.25f);
+                robot.backTankDrive.drive(-2200, -2200, 0.75f);
+                break;
+            default:
+                placeMiddle(robot);
+//                robot.backTankDrive.drive(1000, 1000, 0.5f);
+                break;
+        }
+
+    }
+
+//    public void placeMiddle2(OldRobot robot){
+//        robot.singleJointGripperArm.ungrab();
+//        robot.backTankDrive.drive(-3120, -3120, 0.5f);
+//        robot.singleJointGripperArm.setArm(450);
+//        robot.backTankDrive.drive(500, -500, 0.5f);
+//        robot.backTankDrive.drive(-450, -450, 0.25f);
+//        robot.singleJointGripperArm.grab();
+//        robot.backTankDrive.drive(520, 520, 0.25f);
+//        robot.singleJointGripperArm.setArm(0);
+//        robot.backTankDrive.drive(-375, 375, 0.5f);
+//    }
+
+    public void placeMiddle(OldRobot robot){
+        //First Cone
+        robot.singleJointGripperArm.ungrab();
+        robot.backTankDrive.drive(-3120, -3120, 1f);
+        robot.singleJointGripperArm.setArm(450);
+        robot.backTankDrive.drive(500, -500, 0.6f);
+        robot.backTankDrive.drive(-350, -350, 0.25f);
+        robot.singleJointGripperArm.grab();
+        robot.backTankDrive.drive(500, 500, 0.25f);
+        robot.backTankDrive.drive(-500, 500, 0.6f);
+        robot.singleJointGripperArm.setArm(0);
+
+        //Second Cone
+        robot.backTankDrive.drive(-2300, -2300, 1f);
+        robot.singleJointGripperArm.setArm(125);
+        robot.backTankDrive.drive(-1100, 1100, 0.5f);
+        robot.backTankDrive.drive(-1150, -1150, 0.5f);
+
+        robot.singleJointGripperArm.ungrab();
+        robot.backTankDrive.drive(50, 50, 0.5f);
+        robot.singleJointGripperArm.setArm(300);
+        robot.backTankDrive.drive(-200, 200, 0.6f);
+        robot.backTankDrive.drive(2000,2000, 1f);
+        robot.backTankDrive.drive(-350, 350, 0.5f);
+        robot.singleJointGripperArm.setArm(125);
+        robot.singleJointGripperArm.grab();
+        robot.backTankDrive.drive(400, -400, 0.5f);
+        robot.singleJointGripperArm.setArm(0);
+
+//        robot.backTankDrive.drive(-1900,-1900, 1f);
+//        robot.backTankDrive.drive(50, 50, 1f);
+//        robot.singleJointGripperArm.ungrab();
+//
+//        robot.singleJointGripperArm.setArm(250);
+//        robot.backTankDrive.drive(1175, 1175, 1f);
+//        robot.backTankDrive.drive(500, -500, 0.5f);
+//        robot.singleJointGripperArm.setArm(0);
+//        robot.singleJointGripperArm.ungrab();
+//        robot.singleJointGripperArm.setArm(100);
+//        robot.backTankDrive.drive(-500, 500, 0.5f);
+    }
+
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java
new file mode 100644
index 0000000..06eefa0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.old.Auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
+
+@Disabled
+public class OldRobotAutonomous extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
+
+        //Put motors in encoder mode
+        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.singleJointGripperArm.grab();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+
+            //2080 ticks is 2 rotations
+        //Low Height Pole
+            //robot.singleJointGripperArm.ungrab();
+            //robot.backTankDrive.drive(-400, -400, 0.5f);
+            //robot.backTankDrive.drive(-520, 520, 0.5f);
+            //robot.singleJointGripperArm.setArm(290);
+            //robot.backTankDrive.drive(-200, -200, 0.25f);
+            //robot.singleJointGripperArm.grab();
+            //robot.backTankDrive.drive(520, 520, 0.25f);
+            //robot.singleJointGripperArm.setArm(0);
+            //robot.backTankDrive.drive(375, -375, 0.5f);
+            //robot.backTankDrive.drive(-3120, -3120, 0.5f);
+            //IMPORTANT: TO GO FORWARD PUT NEGATIVE VALUES
+        //Mid Height Pole
+            robot.singleJointGripperArm.ungrab();
+            robot.backTankDrive.drive(-3120, -3120, 0.5f);
+            robot.singleJointGripperArm.setArm(450);
+            robot.backTankDrive.drive(-520, 520, 0.5f);
+            robot.backTankDrive.drive(-400, -400, 0.25f);
+            robot.singleJointGripperArm.grab();
+            robot.backTankDrive.drive(520, 520, 0.25f);
+            robot.singleJointGripperArm.setArm(0);
+            robot.backTankDrive.drive(375, -375, 0.5f);
+            robot.backTankDrive.drive(-400, -400, 0.5f);
+    }
+
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java
new file mode 100644
index 0000000..96d78f0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java
@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.old.Auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
+
+@Disabled
+public class OldRobotAutonomousOther extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
+
+        //Put motors in encoder mode
+        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.singleJointGripperArm.grab();
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+
+        //2080 ticks is 2 rotations
+//        robot.singleJointGripperArm.ungrab();
+//        robot.backTankDrive.drive(-400, -400, 0.5f);
+//        robot.backTankDrive.drive(520, -520, 0.5f);
+//        robot.singleJointGripperArm.setArm(290);
+//        robot.backTankDrive.drive(-200, -200, 0.25f);
+//        robot.singleJointGripperArm.grab();
+//        robot.backTankDrive.drive(520, 520, 0.25f);
+//        robot.singleJointGripperArm.setArm(0);
+//        robot.backTankDrive.drive(-375, 375, 0.5f);
+//        robot.backTankDrive.drive(-3120, -3120, 0.5f);
+//          IMPORTANT: TO GO FORWARD PUT NEGATIVE VALUES
+
+//Mid Height Pole
+        robot.singleJointGripperArm.ungrab();
+        robot.backTankDrive.drive(-3120, -3120, 0.5f);
+        robot.singleJointGripperArm.setArm(450);
+        robot.backTankDrive.drive(520, -520, 0.5f);
+        robot.backTankDrive.drive(-400, -400, 0.25f);
+        robot.singleJointGripperArm.grab();
+        robot.backTankDrive.drive(520, 520, 0.25f);
+        robot.singleJointGripperArm.setArm(0);
+        robot.backTankDrive.drive(-375, 375, 0.5f);
+        robot.backTankDrive.drive(-400, -400, 0.5f);
+    }
+
+
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java
new file mode 100644
index 0000000..c281541
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java
@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.old.older.Auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.Robots.NewRobot;
+
+@Disabled
+public class StrafeBotAutoLeft extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        NewRobot robot = new NewRobot(this, hardwareMap, telemetry, gamepad1);
+        //Put motors in encoder mode
+        robot.strafeDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.strafeDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.strafeDrive.frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.strafeDrive.frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//        robot.linearSlide.slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        //You need to have wait for start or else bad things happen
+        waitForStart();
+        //1350 is Approximately 25 inches moving forwards
+        //1350 is 24 inches moving sideways
+        //270 rotation is approximately 35 degrees
+
+        //Directions
+        //strafe 39.5 inches
+        //place cone
+        //strafe 12 inches
+        //rotate 180 degrees
+        //move forwards 27 inches
+
+//        robot.strafeDrive.strafe(-2221, 0.5f);
+//        robot.strafeDrive.drive(162, 162, 0.5f);
+//        robot.strafeDrive.drive(-162, -162, 0.5f);
+//        robot.strafeDrive.strafe(675, 0.5f);
+//        robot.linearSlide.grab();
+        robot.strafeDrive.strafein(-39.5, 0.5f);
+        robot.strafeDrive.strafe(200, 0.5f);
+//        robot.linearSlide.setSlide(100);
+//        robot.linearSlide.ungrab();
+//        robot.linearSlide.setSlide(0);
+        robot.strafeDrive.strafein(-12, 0.5f);
+        robot.strafeDrive.drive(-926, 926, 0.5f);
+        robot.strafeDrive.strafein(-27, 0.5f);
+
+//        robot.strafeDrive.drive(1680, 1680, 0.5f);
+//        robot.strafeDrive.strafe(1000, 0.5f);
+//        robot.strafeDrive.strafe(-1000, 0.5f);
+//        robot.strafeDrive.drive(1000, -1000, 0.5f);
+//        robot.strafeDrive.drive(-520,-520, 0.5f);
+//        robot.strafeDrive.drive(-520,-520, -0.5f);
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java
new file mode 100644
index 0000000..3384481
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java
@@ -0,0 +1,84 @@
+package org.firstinspires.ftc.teamcode.old.Robots;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.old.Subsystems.StrafeDrive;
+import org.firstinspires.ftc.teamcode.old.Subsystems.LinearSlide;
+
+public class NewRobot {
+    private final HardwareMap hardwareMap;
+    private final Telemetry telemetry;
+
+    // Hardware Configuration Names
+    public static final String FRONT_LEFT_MOTOR_NAME = "motor_front_left";
+    public static final String FRONT_RIGHT_MOTOR_NAME = "motor_front_right";
+    public static final String BACK_LEFT_MOTOR_NAME = "motor_back_left";
+    public static final String BACK_RIGHT_MOTOR_NAME = "motor_back_right";
+    public static final String LINEAR_SLIDE_MOTOR_NAME = "motor_slide";
+    public static final String GRIPPER1_SERVO_NAME = "grip1";
+    public static final String GRIPPER2_SERVO_NAME = "grip2";
+
+    // Subsystems
+    public final StrafeDrive strafeDrive;
+    public final LinearOpMode linearOpMode;
+    public final LinearSlide linearSlide;
+
+    // Sensors
+    public final Gamepad gamepad;
+
+    public NewRobot(LinearOpMode linearOpMode, HardwareMap hardwareMap, Telemetry telemetry, Gamepad gamepad) {
+        this.hardwareMap = hardwareMap;
+        this.telemetry = telemetry;
+        this.linearOpMode = linearOpMode;
+        this.gamepad = gamepad;
+        this.strafeDrive = initStrafeDrive();
+        this.linearSlide = initLinearSlide();
+    }
+    
+    public StrafeDrive initStrafeDrive() {
+        DcMotor frontLeftMotor = (DcMotor) hardwareMap.get(FRONT_LEFT_MOTOR_NAME);
+        DcMotor frontRightMotor = (DcMotor) hardwareMap.get(FRONT_RIGHT_MOTOR_NAME);
+        DcMotor backLeftMotor = (DcMotor) hardwareMap.get(BACK_LEFT_MOTOR_NAME);
+        DcMotor backRightMotor = (DcMotor) hardwareMap.get(BACK_RIGHT_MOTOR_NAME);
+
+        backLeftMotor.setDirection(DcMotor.Direction.REVERSE);
+        frontLeftMotor.setDirection(DcMotor.Direction.REVERSE);
+
+        return new StrafeDrive(linearOpMode,
+                telemetry,
+                frontLeftMotor,
+                frontRightMotor,
+                backLeftMotor,
+                backRightMotor,
+                gamepad
+        );
+    }
+
+    public LinearSlide initLinearSlide(){
+        DcMotor slideMotor = (DcMotor) hardwareMap.get(LINEAR_SLIDE_MOTOR_NAME);
+        slideMotor.setTargetPosition(0);
+        slideMotor.setPower(1);
+        slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slideMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+        Servo gripper1Servo = (Servo) hardwareMap.get(GRIPPER1_SERVO_NAME);
+        Servo gripper2Servo = (Servo) hardwareMap.get(GRIPPER2_SERVO_NAME);
+        return new LinearSlide(
+                linearOpMode,
+                telemetry,
+                slideMotor,
+                gripper1Servo,
+                gripper2Servo,
+                gamepad,
+                450,
+                0,
+                0.45,
+                1
+                );
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java
new file mode 100644
index 0000000..e99edee
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java
@@ -0,0 +1,83 @@
+package org.firstinspires.ftc.teamcode.old.older.Robots;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.old.older.Subsystems.BackTankDrive;
+import org.firstinspires.ftc.teamcode.old.Subsystems.SingleJointGripperArm;
+
+public class OldRobot {
+    private final HardwareMap hardwareMap;
+    private final Telemetry telemetry;
+
+    // Hardware Configuration Names
+    public static final String BACK_LEFT_MOTOR_NAME = "b_left";
+    public static final String BACK_RIGHT_MOTOR_NAME = "b_right";
+    public static final String ARM_MOTOR_NAME = "arm";
+    public static final String GRIPPER1_SERVO_NAME = "grip1";
+    public static final String GRIPPER2_SERVO_NAME = "grip2";
+
+    // Subsystems
+    public final BackTankDrive backTankDrive;
+    public final SingleJointGripperArm singleJointGripperArm;
+    public final LinearOpMode linearOpMode;
+
+    // Sensors
+    public final Gamepad gamepad;
+
+    public OldRobot(LinearOpMode linearOpMode, HardwareMap hardwareMap, Telemetry telemetry, Gamepad gamepad) {
+        this.linearOpMode = linearOpMode;
+        this.hardwareMap = hardwareMap;
+        this.telemetry = telemetry;
+        this.gamepad = gamepad;
+        this.backTankDrive = initBackTankDrive();
+        this.singleJointGripperArm = initSingleJointGripperArm();
+    }
+
+    public BackTankDrive initBackTankDrive() {
+        DcMotor backLeftMotor = (DcMotor) hardwareMap.get(BACK_LEFT_MOTOR_NAME);
+        DcMotor backRightMotor = (DcMotor) hardwareMap.get(BACK_RIGHT_MOTOR_NAME);
+
+        backRightMotor.setDirection(DcMotor.Direction.REVERSE);
+
+        return new BackTankDrive(
+                linearOpMode,
+                telemetry,
+                backLeftMotor,
+                backRightMotor,
+                gamepad,
+                1,
+                5000, // TODO: get what this value is in class
+                0.1 // TODO: measure in class what this value is
+        );
+    }
+
+    public SingleJointGripperArm initSingleJointGripperArm() {
+        DcMotor armMotor = (DcMotor) hardwareMap.get(ARM_MOTOR_NAME);
+
+        armMotor.setTargetPosition(0);
+        armMotor.setPower(1);
+        armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+        Servo gripper1Servo = (Servo) hardwareMap.get(GRIPPER1_SERVO_NAME);
+        Servo gripper2Servo = (Servo) hardwareMap.get(GRIPPER2_SERVO_NAME);
+
+        return new SingleJointGripperArm(
+                linearOpMode,
+                telemetry,
+                armMotor,
+                gripper1Servo,
+                gripper2Servo,
+                gamepad,
+                0.45,
+                1,
+                0,
+                450
+        );
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java
new file mode 100644
index 0000000..3f30df0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java
@@ -0,0 +1,79 @@
+package org.firstinspires.ftc.teamcode.old.older.Subsystems;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public class BackTankDrive {
+    private final Telemetry telemetry;
+    public final DcMotor backLeftMotor;
+    public final DcMotor backRightMotor;
+    public final Gamepad gamepad;
+    public final int turnSensitivity;
+    public final int encoderSteps; // TODO: find out what this number is
+    public final double wheelRadius; // in meters
+    public int leftPos = 0;
+    public int rightPos = 0;
+    private LinearOpMode opMode;
+
+    public BackTankDrive(LinearOpMode opmode, Telemetry telemetry, DcMotor backLeftMotor, DcMotor backRightMotor, Gamepad gamepad, int turnSensitivity, int encoderSteps, double wheelRadius) {
+        this.opMode = opmode;
+        this.telemetry = telemetry;
+        this.backLeftMotor = backLeftMotor;
+        this.backRightMotor = backRightMotor;
+        this.gamepad = gamepad;
+        this.turnSensitivity = turnSensitivity;
+        this.encoderSteps = encoderSteps;
+        this.wheelRadius = wheelRadius;
+    }
+
+    public void respondToGamepad() {
+        if (gamepad.right_stick_x < 0.01 && gamepad.right_stick_x > -0.01 && (gamepad.left_stick_y > 0.01 || gamepad.left_stick_y < -0.01)) {
+            // Left-stick control all, going straight
+            backLeftMotor.setPower(gamepad.left_stick_y / 1.5);
+            backRightMotor.setPower(gamepad.left_stick_y / 1.5);
+        } else if (gamepad.right_stick_x < -0.01 && (gamepad.left_stick_y > 0.05 || gamepad.left_stick_y < -0.01)) {
+            // Turning left while moving
+            backLeftMotor.setPower((gamepad.left_stick_y / turnSensitivity) / 2);
+            backRightMotor.setPower(gamepad.left_stick_y / 2);
+        } else if (gamepad.right_stick_x > 0.01 && (gamepad.left_stick_y > 0.05 || gamepad.left_stick_y < -0.01)) {
+            // Turning right wile moving forward
+            backLeftMotor.setPower(gamepad.left_stick_y / 2);
+            backRightMotor.setPower((gamepad.left_stick_y / turnSensitivity) / 2);
+        } else if (gamepad.right_stick_x < -0.01 && (gamepad.left_stick_y > -0.05 || gamepad.left_stick_y < 0.01)) {
+            // Turning left without going forward
+            backRightMotor.setPower(gamepad.right_stick_x / 2);
+            backLeftMotor.setPower(-(gamepad.right_stick_x / 2));
+        } else if (gamepad.right_stick_x > 0.01 && (gamepad.left_stick_y > -0.05 || gamepad.left_stick_y < 0.01)) {
+            // Turning right without moving forward
+            backLeftMotor.setPower(-(gamepad.right_stick_x / 2));
+            backRightMotor.setPower(gamepad.right_stick_x / 2);
+        } else if (gamepad.right_stick_x == 0 && gamepad.left_stick_y == 0) {
+            backLeftMotor.setPower(0);
+            backRightMotor.setPower(0);
+        } else if (gamepad.right_bumper || gamepad.left_bumper) {
+            //covered in gamepadPlusCamera in CameraRobotTeleOp class
+        } else {
+            backLeftMotor.setPower(0);
+            backRightMotor.setPower(0);
+        }
+    }
+
+
+    public void drive(int leftmove, int rightmove, float speed) {
+        leftPos += leftmove;
+        rightPos += rightmove;
+        backLeftMotor.setTargetPosition(leftPos);
+        backRightMotor.setTargetPosition(rightPos);
+        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backLeftMotor.setPower(speed);
+        backRightMotor.setPower(speed);
+
+        while (opMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy()) {
+            opMode.idle();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java
new file mode 100644
index 0000000..13ef60d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java
@@ -0,0 +1,185 @@
+package org.firstinspires.ftc.teamcode.old.older.Subsystems;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class BoeDetection extends OpenCvPipeline {
+    /*
+    YELLOW  = Parking Left
+    CYAN    = Parking Middle
+    MAGENTA = Parking Right
+     */
+
+    public enum Cone {
+        RED,
+        BLUE,
+        UNDECIDED
+    }
+
+    public enum Distance {
+        CLOSE,
+        MID,
+        FAR,
+        UNSEEN
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static final int X = 55;
+    private static final int Y = 120;
+    private static final int STEP = 10;
+    private static final int WIDTH = 25;
+    private static final int HEIGHT = 15;
+    private static final Point FAR_TOPLEFT_ANCHOR_POINT = new Point(X, Y);
+    private static final Point CLOSE_TOPLEFT_ANCHOR_POINT = new Point(X - (2 * STEP), Y - (2 * STEP));
+
+    // Width and height for the bounding box
+    public static int FAR_REGION_WIDTH = WIDTH;
+    public static int FAR_REGION_HEIGHT = HEIGHT;
+    public static int MID_REGION_WIDTH = WIDTH + STEP + STEP;
+    public static int MID_REGION_HEIGHT = HEIGHT + STEP + STEP;
+    public static int CLOSE_REGION_WIDTH = WIDTH + STEP + STEP + STEP + STEP;
+    public static int CLOSE_REGION_HEIGHT = HEIGHT + STEP + STEP + STEP + STEP;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_red_bounds = new Scalar(100, 0, 0, 255),
+            upper_red_bounds = new Scalar(255, 150, 150, 255),
+            lower_blue_bounds = new Scalar(0, 0, 100, 255),
+            upper_blue_bounds = new Scalar(150, 150, 255, 255);
+
+    // Color definitions
+    private final Scalar
+            RED = new Scalar(255, 0, 0),
+            BLUE = new Scalar(0, 0, 255),
+            BLACK = new Scalar(0, 0, 0);
+
+    // Percent and mat definitions
+    private double redPercentC, bluePercentC, redPercentM, bluePercentM, redPercentF, bluePercentF;
+    private final Mat redMatC = new Mat();
+    private final Mat blueMatC = new Mat();
+    private Mat redMatF = new Mat();
+    private Mat blueMatF = new Mat();
+    private Mat redMatM = new Mat();
+    private Mat blueMatM = new Mat();
+    private Mat blurredMatClose = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point((2 * STEP), (2 * STEP));
+    Point sleeve_pointB = new Point(
+            (2 * STEP) + FAR_REGION_WIDTH,
+            (2 * STEP) + FAR_REGION_HEIGHT);
+
+    Point sleeve_pointC = new Point(STEP, STEP);
+    Point sleeve_pointD = new Point(
+            STEP + MID_REGION_WIDTH,
+            STEP + MID_REGION_HEIGHT);
+
+    Point sleeve_pointE = new Point(
+            CLOSE_TOPLEFT_ANCHOR_POINT.x,
+            CLOSE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointF = new Point(
+            CLOSE_TOPLEFT_ANCHOR_POINT.x + CLOSE_REGION_WIDTH,
+            CLOSE_TOPLEFT_ANCHOR_POINT.y + CLOSE_REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile Cone cone = Cone.UNDECIDED;
+    private volatile Distance distance = Distance.UNSEEN;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMatClose, new Size(5, 5));
+        blurredMatClose = blurredMatClose.submat(new Rect(sleeve_pointE, sleeve_pointF));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMatClose, blurredMatClose, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMatClose, lower_red_bounds, upper_red_bounds, redMatC);
+        Core.inRange(blurredMatClose, lower_blue_bounds, upper_blue_bounds, blueMatC);
+
+        redMatM = redMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
+        blueMatM = blueMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
+
+        redMatF = redMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
+        blueMatF = blueMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+
+        // Gets color specific values
+        redPercentC = Core.countNonZero(redMatC);
+        bluePercentC = Core.countNonZero(blueMatC);
+
+        redPercentM = Core.countNonZero(redMatM);
+        bluePercentM = Core.countNonZero(blueMatM);
+
+        redPercentF = Core.countNonZero(redMatF);
+        bluePercentF = Core.countNonZero(blueMatF);
+
+
+        // Calculates the highest amount of pixels being covered on each side
+        double maxPercent = Math.max(redPercentF, bluePercentF);
+
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (maxPercent == redPercentF) {
+            if (redPercentC >= 150) {
+                cone = Cone.RED;
+                distance = Distance.CLOSE;
+            } else if (redPercentM >= 100) {
+                cone = Cone.RED;
+                distance = Distance.MID;
+            } else if (redPercentF >= 50) {
+                cone = Cone.RED;
+                distance = Distance.FAR;
+            } else {
+                cone = Cone.UNDECIDED;
+                distance = Distance.UNSEEN;
+            }
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    RED,
+                    2
+            );
+        } else if (maxPercent == bluePercentC && bluePercentC >= 50) {
+            cone = Cone.BLUE;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    BLUE,
+                    2
+            );
+        } else {
+            cone = Cone.UNDECIDED;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    BLACK,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        redMatC.release();
+        blueMatC.release();
+        kernel.release();
+
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public Cone getPosition() {
+        return cone;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java
new file mode 100644
index 0000000..7c560e8
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java
@@ -0,0 +1,170 @@
+package org.firstinspires.ftc.teamcode.old.Subsystems;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class FullDetection extends OpenCvPipeline {
+    /*
+    YELLOW  = Parking Left
+    CYAN    = Parking Middle
+    MAGENTA = Parking Right
+     */
+
+    public enum ParkingPosition {
+        LEFT,
+        CENTER,
+        RIGHT
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(55, 120);
+    private static Point POLE_TOPLEFT_ANCHOR_POINT = new Point(75, 100);
+
+    // Width and height for the bounding box
+    public static int SLEEVE_REGION_WIDTH = 25;
+    public static int SLEEVE_REGION_HEIGHT = 15;
+    public static int POLE_REGION_WIDTH = 25;
+    public static int POLE_REGION_HEIGHT = 15;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_yellow_bounds  = new Scalar(150, 150, 0, 255),
+            upper_yellow_bounds  = new Scalar(255, 255, 150, 255),
+            lower_cyan_bounds    = new Scalar(0, 120, 120, 255),
+            upper_cyan_bounds    = new Scalar(150, 255, 255, 255),
+            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
+            upper_magenta_bounds = new Scalar(255, 170, 255, 255),
+            lower_pole_bounds = new Scalar(150, 150, 0, 255),
+            upper_pole_bounds = new Scalar(255, 255, 150, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW  = new Scalar(255, 255, 0),
+            CYAN    = new Scalar(0, 255, 255),
+            MAGENTA = new Scalar(255, 0, 255);
+
+    // Percent and mat definitions
+    private double yelPercentS, cyaPercentS, magPercentS, yelPercentP;
+    private Mat yelMatS = new Mat(), cyaMatS = new Mat(), magMatS = new Mat(), yelMatP = new Mat(), blurredMat = new Mat(), sleeveMat = new Mat(), poleMat = new Mat(), kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + SLEEVE_REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + SLEEVE_REGION_HEIGHT);
+    Point pole_pointA = new Point(
+            POLE_TOPLEFT_ANCHOR_POINT.x,
+            POLE_TOPLEFT_ANCHOR_POINT.y);
+    Point pole_pointB = new Point(
+            POLE_TOPLEFT_ANCHOR_POINT.x + POLE_REGION_WIDTH,
+            POLE_TOPLEFT_ANCHOR_POINT.y + POLE_REGION_HEIGHT);
+
+    // Running variable storing the parking position and pole detection
+    private volatile ParkingPosition position = ParkingPosition.LEFT;
+    private volatile boolean isPole = false;
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        sleeveMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+        poleMat = blurredMat.submat(new Rect(pole_pointA, pole_pointB));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(sleeveMat, sleeveMat, Imgproc.MORPH_CLOSE, kernel);
+        Imgproc.morphologyEx(poleMat, poleMat, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(sleeveMat, lower_yellow_bounds, upper_yellow_bounds, yelMatS);
+        Core.inRange(sleeveMat, lower_cyan_bounds, upper_cyan_bounds, cyaMatS);
+        Core.inRange(sleeveMat, lower_magenta_bounds, upper_magenta_bounds, magMatS);
+        Core.inRange(poleMat, lower_pole_bounds, upper_pole_bounds, yelMatP);
+
+        // Gets color specific values
+        yelPercentS = Core.countNonZero(yelMatS);
+        cyaPercentS = Core.countNonZero(cyaMatS);
+        magPercentS = Core.countNonZero(magMatS);
+        yelPercentP = Core.countNonZero(yelMatP);
+
+        // Calculates the highest amount of pixels being covered on each side
+        double maxPercent = Math.max(yelPercentS, Math.max(cyaPercentS, magPercentS));
+
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (maxPercent == yelPercentS) {
+            position = ParkingPosition.LEFT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        } else if (maxPercent == cyaPercentS) {
+            position = ParkingPosition.CENTER;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        } else if (maxPercent == magPercentS) {
+            position = ParkingPosition.RIGHT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    MAGENTA,
+                    2
+            );
+        }
+        if (yelPercentP >= 60) {
+            isPole = true;
+            Imgproc.rectangle(
+                    input,
+                    pole_pointA,
+                    pole_pointB,
+                    YELLOW,
+                    2
+            );
+        } else {
+            isPole = false;
+            Imgproc.rectangle(
+                    input,
+                    pole_pointA,
+                    pole_pointB,
+                    CYAN,
+                    2
+            );
+        }
+        // Memory cleanup
+        blurredMat.release();
+        yelMatS.release();
+        cyaMatS.release();
+        magMatS.release();
+        yelMatP.release();
+        kernel.release();
+
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public ParkingPosition getPosition() {
+        return position;
+    }
+    public boolean getPole() {
+        return isPole;
+    }
+    public double getPercent() {
+        return yelPercentP;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java
new file mode 100644
index 0000000..705e63b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java
@@ -0,0 +1,102 @@
+package org.firstinspires.ftc.teamcode.old.Subsystems;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.old.Util.Maths;
+
+public class LinearSlide {
+    private final Telemetry telemetry;
+    public final DcMotor slideMotor;
+    public final Servo gripper1Servo;
+    public final Servo gripper2Servo;
+    public final Gamepad gamepad;
+    public final int minHeight;
+    public final int maxHeight;
+    public final double gripMin;
+    public final double gripMax;
+    private double gripPos = 1;
+    private double height = 0;
+    private int armMotorSteps =0;
+    private boolean dpadpressed = false;
+    private LinearOpMode linearOpMode;
+
+    public LinearSlide(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor slideMotor, Servo gripper1, Servo gripper2, Gamepad gamepad, int maxHeight, int minHeight, double gripMin, double gripMax){
+        this.linearOpMode = linearOpMode;
+        this.telemetry = telemetry;
+        this.slideMotor = slideMotor;
+        this.gripper1Servo = gripper1;
+        this.gripper2Servo = gripper2;
+        this.gamepad = gamepad;
+        this.minHeight = minHeight;
+        this.maxHeight = maxHeight;
+        this.gripMin = gripMin;
+        this.gripMax = gripMax;
+    }
+
+    public void respondToGamepad() {
+        slideRespondToGamepad();
+        gripRespondToGamepad();
+    }
+
+    public void gripRespondToGamepad() {
+        if (gamepad.right_trigger > 0.5) {
+            gripPos = gripMin;
+        } else if (gamepad.left_trigger > 0.5) {
+            gripPos = gripMax;
+        }
+
+        gripPos = Maths.clamp(gripPos, gripMin, gripMax);
+
+        gripper1Servo.setPosition(1 - gripPos);
+        gripper2Servo.setPosition(gripPos);
+    }
+
+    public void slideRespondToGamepad(){
+        if (gamepad.a) {
+            armMotorSteps = 0;
+        } else if (gamepad.b) {
+            armMotorSteps = 86;
+        } else if (gamepad.x) {
+            armMotorSteps = 270;
+        } else if (gamepad.y) {
+            armMotorSteps = 460;
+        }
+
+        // TODO: check if this sensitivity is right
+        if (gamepad.dpad_up) {
+            armMotorSteps += 4;
+            dpadpressed = true;
+        } else if (gamepad.dpad_down) {
+            armMotorSteps -= 4;
+            dpadpressed = true;
+        } else if (dpadpressed) {
+            armMotorSteps = slideMotor.getCurrentPosition();
+            dpadpressed = false;
+        }
+
+        armMotorSteps = Maths.clamp(armMotorSteps, minHeight, maxHeight);
+
+        slideMotor.setTargetPosition(armMotorSteps);
+    }
+
+    public void grab() {
+        gripper1Servo.setPosition(1);
+        gripper2Servo.setPosition(0);
+    }
+
+    public void ungrab() {
+        gripper1Servo.setPosition(gripMin);
+        gripper2Servo.setPosition(1-gripMin);
+    }
+
+    public void setSlide(int steps) {
+        slideMotor.setTargetPosition(Maths.clamp(steps, minHeight, maxHeight));
+        while (linearOpMode.opModeIsActive() && slideMotor.isBusy()) {
+            linearOpMode.idle();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java
new file mode 100644
index 0000000..a395cc2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java
@@ -0,0 +1,84 @@
+package org.firstinspires.ftc.teamcode.old.Subsystems;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class NewPoleDetection extends OpenCvPipeline {
+
+
+    private volatile boolean isPole = false;
+
+    // TOPLEFT anchor point for the bounding box
+    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(45, 60);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 25;
+    public static int REGION_HEIGHT = 15;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_yellow_bounds  = new Scalar(150, 150, 0, 255),
+            upper_yellow_bounds  = new Scalar(255, 255, 150, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW  = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255);
+
+    // Percent and mat definitions
+    private double yelPercent;
+    private Mat yelMat = new Mat(), blurredMat = new Mat(), kernel = new Mat(), poleEdge = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        //Black and white
+        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+
+
+        // Gets color specific values
+        yelPercent = Core.countNonZero(yelMat);
+
+        // Calculates the highest amount of pixels being covered on each side
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (yelPercent >= 60) {
+            isPole = true;
+        } else {
+        }
+
+        // Memory cleanup
+        blurredMat.release();
+        yelMat.release();
+        kernel.release();
+
+        return input;
+    }
+
+    public boolean getPole() {
+        return isPole;
+    }
+    public double getPercent() {
+        return yelPercent;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java
new file mode 100644
index 0000000..088a4eb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java
@@ -0,0 +1,100 @@
+package org.firstinspires.ftc.teamcode.old.Subsystems;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class PoleDetection extends OpenCvPipeline {
+    private volatile boolean isPole = false;
+
+    // TOPLEFT anchor point for the bounding box
+    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(45, 60);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 25;
+    public static int REGION_HEIGHT = 15;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
+            upper_yellow_bounds = new Scalar(255, 255, 150, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255);
+
+    // Percent and mat definitions
+    private double yelPercent;
+    private final Mat yelMat = new Mat();
+    private Mat blurredMat = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+
+        // Gets color specific values
+        yelPercent = Core.countNonZero(yelMat);
+
+        // Calculates the highest amount of pixels being covered on each side
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (yelPercent >= 60) {
+            isPole = true;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        } else {
+            isPole = false;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        blurredMat.release();
+        yelMat.release();
+        kernel.release();
+
+        return input;
+    }
+
+    public boolean getPole() {
+        return isPole;
+    }
+
+    public double getPercent() {
+        return yelPercent;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java
new file mode 100644
index 0000000..12c4b83
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java
@@ -0,0 +1,102 @@
+package org.firstinspires.ftc.teamcode.old.Subsystems;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.old.Util.Maths;
+
+public class SingleJointGripperArm {
+    private final Telemetry telemetry;
+    public final DcMotor armMotor;
+    public final Servo gripper1Servo;
+    public final Servo gripper2Servo;
+    public final Gamepad gamepad;
+    public final double gripMin;
+    public final double gripMax;
+    public final int armMin;
+    public final int armMax;
+    private int armMotorSteps = 0;
+    private double gripPos = 1;
+    private LinearOpMode linearOpMode;
+    private boolean dpadpressed = false;
+
+    public SingleJointGripperArm(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor armMotor, Servo gripper1Servo, Servo gripper2Servo, Gamepad gamepad, double gripMin, double gripMax, int armMin, int armMax) {
+        this.linearOpMode = linearOpMode;
+        this.telemetry = telemetry;
+        this.armMotor = armMotor;
+        this.gripper1Servo = gripper1Servo;
+        this.gripper2Servo = gripper2Servo;
+        this.gamepad = gamepad;
+        this.gripMin = gripMin;
+        this.gripMax = gripMax;
+        this.armMin = armMin;
+        this.armMax = armMax;
+    }
+
+    public void respondToGamepad() {
+        armRespondToGamepad();
+        gripRespondToGamepad();
+    }
+
+    public void gripRespondToGamepad() {
+        if (gamepad.right_trigger > 0.5) {
+            gripPos = gripMin;
+        } else if (gamepad.left_trigger > 0.5) {
+            gripPos = gripMax;
+        }
+
+        gripPos = Maths.clamp(gripPos, gripMin, gripMax);
+
+        gripper1Servo.setPosition(1 - gripPos);
+        gripper2Servo.setPosition(gripPos);
+    }
+
+    public void armRespondToGamepad() {
+        if (gamepad.a) {
+            armMotorSteps = 0;
+        } else if (gamepad.b) {
+            armMotorSteps = 86;
+        } else if (gamepad.x) {
+            armMotorSteps = 270;
+        } else if (gamepad.y) {
+            armMotorSteps = 460;
+        }
+
+        // TODO: check if this sensitivity is right
+        if (gamepad.dpad_up) {
+            armMotorSteps += 4;
+            dpadpressed = true;
+        } else if (gamepad.dpad_down) {
+            armMotorSteps -= 4;
+            dpadpressed = true;
+        } else if (dpadpressed) {
+            armMotorSteps = armMotor.getCurrentPosition();
+            dpadpressed = false;
+        }
+
+        armMotorSteps = Maths.clamp(armMotorSteps, armMin, armMax);
+
+        armMotor.setTargetPosition(armMotorSteps);
+    }
+
+    public void grab() {
+        gripper1Servo.setPosition(0);
+        gripper2Servo.setPosition(1);
+    }
+
+    public void ungrab() {
+        gripper1Servo.setPosition(1-gripMin);
+        gripper2Servo.setPosition(gripMin);
+    }
+
+    public void setArm(int steps) {
+        armMotor.setTargetPosition(Maths.clamp(steps, armMin, armMax));
+        while (linearOpMode.opModeIsActive() && armMotor.isBusy()) {
+            linearOpMode.idle();
+        }
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java
new file mode 100644
index 0000000..379c904
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java
@@ -0,0 +1,134 @@
+package org.firstinspires.ftc.teamcode.old.Subsystems;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class SleeveDetection extends OpenCvPipeline {
+    /*
+    YELLOW  = Parking Left
+    CYAN    = Parking Middle
+    MAGENTA = Parking Right
+     */
+
+    public enum ParkingPosition {
+        LEFT,
+        CENTER,
+        RIGHT
+    }
+
+    // TOPLEFT anchor point for the bounding box
+    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(55, 120);
+
+    // Width and height for the bounding box
+    public static int REGION_WIDTH = 25;
+    public static int REGION_HEIGHT = 15;
+
+    // Lower and upper boundaries for colors
+    private static final Scalar
+            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
+            upper_yellow_bounds = new Scalar(255, 255, 150, 255),
+            lower_cyan_bounds = new Scalar(0, 120, 120, 255),
+            upper_cyan_bounds = new Scalar(150, 255, 255, 255),
+            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
+            upper_magenta_bounds = new Scalar(255, 170, 255, 255);
+
+    // Color definitions
+    private final Scalar
+            YELLOW = new Scalar(255, 255, 0),
+            CYAN = new Scalar(0, 255, 255),
+            MAGENTA = new Scalar(255, 0, 255);
+
+    // Percent and mat definitions
+    private double yelPercent, cyaPercent, magPercent;
+    private final Mat yelMat = new Mat();
+    private final Mat cyaMat = new Mat();
+    private final Mat magMat = new Mat();
+    private Mat blurredMat = new Mat();
+    private Mat kernel = new Mat();
+
+    // Anchor point definitions
+    Point sleeve_pointA = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
+    Point sleeve_pointB = new Point(
+            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
+            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
+
+    // Running variable storing the parking position
+    private volatile ParkingPosition position = ParkingPosition.LEFT;
+
+    @Override
+    public Mat processFrame(Mat input) {
+        // Noise reduction
+        Imgproc.blur(input, blurredMat, new Size(5, 5));
+        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
+
+        // Apply Morphology
+        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
+        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
+
+        // Gets channels from given source mat
+        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
+        Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
+        Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
+
+        // Gets color specific values
+        yelPercent = Core.countNonZero(yelMat);
+        cyaPercent = Core.countNonZero(cyaMat);
+        magPercent = Core.countNonZero(magMat);
+
+        // Calculates the highest amount of pixels being covered on each side
+        double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
+
+        // Checks all percentages, will highlight bounding box in camera preview
+        // based on what color is being detected
+        if (maxPercent == yelPercent) {
+            position = ParkingPosition.LEFT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    YELLOW,
+                    2
+            );
+        } else if (maxPercent == cyaPercent) {
+            position = ParkingPosition.CENTER;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    CYAN,
+                    2
+            );
+        } else if (maxPercent == magPercent) {
+            position = ParkingPosition.RIGHT;
+            Imgproc.rectangle(
+                    input,
+                    sleeve_pointA,
+                    sleeve_pointB,
+                    MAGENTA,
+                    2
+            );
+        }
+
+        // Memory cleanup
+        blurredMat.release();
+        yelMat.release();
+        cyaMat.release();
+        magMat.release();
+        kernel.release();
+
+        return input;
+    }
+
+    // Returns an enum being the current position where the robot will park
+    public ParkingPosition getPosition() {
+        return position;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java
new file mode 100644
index 0000000..c4b6140
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java
@@ -0,0 +1,176 @@
+package org.firstinspires.ftc.teamcode.old.older.Subsystems;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import org.firstinspires.ftc.teamcode.old.Util.Measure;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public class StrafeDrive {
+    private final Telemetry telemetry;
+    public final DcMotor frontLeftMotor;
+    public final DcMotor frontRightMotor;
+    public final DcMotor backLeftMotor;
+    public final DcMotor backRightMotor;
+    public final LinearOpMode linearOpMode;
+    public final Gamepad gamepad;
+    public int flPos = 0;
+    public int frPos = 0;
+    public int blPos = 0;
+    public int brPos = 0;
+
+    public StrafeDrive(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor frontLeftMotor, DcMotor frontRightMotor, DcMotor backLeftMotor, DcMotor backRightMotor, Gamepad gamepad) {
+        this.telemetry = telemetry;
+        this.linearOpMode = linearOpMode;
+        this.frontLeftMotor = frontLeftMotor;
+        this.frontRightMotor = frontRightMotor;
+        this.backLeftMotor = backLeftMotor;
+        this.backRightMotor = backRightMotor;
+        this.gamepad = gamepad;
+    }
+
+
+    public void respondToGamepad() {
+
+        double forward = -gamepad.left_stick_y;
+        double strafe = gamepad.left_stick_x;
+        double rotate = gamepad.right_stick_x;
+        if (Math.abs(-gamepad.left_stick_y) < 0.01) {
+            forward = 0;
+        }
+
+        if (Math.abs(gamepad.left_stick_x) < 0.01) {
+            strafe = 0;
+        }
+
+        if (Math.abs(gamepad.right_stick_x) < 0.01) {
+            rotate = 0;
+        }
+        frontLeftMotor.setPower(forward + strafe + rotate);
+        backLeftMotor.setPower(forward - strafe + rotate);
+        frontRightMotor.setPower(forward + strafe - rotate);
+        backRightMotor.setPower(forward - strafe - rotate);
+    }
+
+    public void drive(int leftMove, int rightMove, float speed) {
+
+        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        flPos = leftMove;
+        frPos = rightMove;
+        blPos = leftMove;
+        brPos = rightMove;
+
+
+        backLeftMotor.setTargetPosition(blPos);
+        frontRightMotor.setTargetPosition(frPos);
+        backRightMotor.setTargetPosition(brPos);
+        frontLeftMotor.setTargetPosition(flPos);
+        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backLeftMotor.setPower(speed);
+        backRightMotor.setPower(speed);
+        frontLeftMotor.setPower(speed);
+        frontRightMotor.setPower(speed);
+
+        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
+            linearOpMode.idle();
+        }
+    }
+
+    public void drivein(double leftinches, int rightinches, float speed) {
+
+        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        flPos = Measure.sTicks(leftinches);
+        frPos = Measure.sTicks(rightinches);
+        blPos = Measure.sTicks(leftinches);
+        brPos = Measure.sTicks(rightinches);
+
+
+        backLeftMotor.setTargetPosition(blPos);
+        frontRightMotor.setTargetPosition(frPos);
+        backRightMotor.setTargetPosition(brPos);
+        frontLeftMotor.setTargetPosition(flPos);
+        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backLeftMotor.setPower(speed);
+        backRightMotor.setPower(speed);
+        frontLeftMotor.setPower(speed);
+        frontRightMotor.setPower(speed);
+
+        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
+            linearOpMode.idle();
+        }
+    }
+
+    public void strafe(int move, float speed) {
+
+        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        flPos = move;
+        frPos = move;
+        blPos = move * -1;
+        brPos = move * -1;
+        backLeftMotor.setTargetPosition(blPos);
+        frontRightMotor.setTargetPosition(frPos);
+        backRightMotor.setTargetPosition(brPos);
+        frontLeftMotor.setTargetPosition(flPos);
+        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backLeftMotor.setPower(speed);
+        backRightMotor.setPower(speed);
+        frontLeftMotor.setPower(speed);
+        frontRightMotor.setPower(speed);
+
+        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
+            linearOpMode.idle();
+        }
+    }
+
+    public void strafein(double inches, float speed) {
+
+        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        flPos = Measure.sTicks(inches);
+        frPos = Measure.sTicks(inches);
+        blPos = Measure.sTicks(inches) * -1;
+        brPos = Measure.sTicks(inches) * -1;
+        backLeftMotor.setTargetPosition(blPos);
+        frontRightMotor.setTargetPosition(frPos);
+        backRightMotor.setTargetPosition(brPos);
+        frontLeftMotor.setTargetPosition(flPos);
+        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        backLeftMotor.setPower(speed);
+        backRightMotor.setPower(speed);
+        frontLeftMotor.setPower(speed);
+        frontRightMotor.setPower(speed);
+
+        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
+            linearOpMode.idle();
+        }
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java
new file mode 100644
index 0000000..20f8fc6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java
@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode.old.older.TeleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.old.older.Robots.OldRobot;
+import org.firstinspires.ftc.teamcode.old.Subsystems.PoleDetection;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+@Disabled
+public class CameraRobotTeleOp extends LinearOpMode {
+
+    PoleDetection poleDetection = new PoleDetection();
+    OpenCvCamera camera;
+    String webcamName = "Webcam 1";
+
+    @Override
+    public void runOpMode() {
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
+        poleDetection = new PoleDetection();
+        camera.setPipeline(poleDetection);
+
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            @Override
+            public void onOpened() {
+                camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT);
+            }
+
+            @Override
+            public void onError(int errorCode) {
+            }
+        });
+
+        while (!isStarted()) {
+            telemetry.addData("Is there a pole?: ", poleDetection.getPole());
+            telemetry.addData("Percent Yellow: ", poleDetection.getPercent());
+            telemetry.update();
+        }
+        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            gamepadPlusCamera(robot, poleDetection);
+            robot.singleJointGripperArm.respondToGamepad();
+            telemetry.addData("Is there a pole?: ", poleDetection.getPole());
+            telemetry.addData("Percent Yellow: ", poleDetection.getPercent());
+            telemetry.update();
+        }
+    }
+
+    private void gamepadPlusCamera(OldRobot robot, PoleDetection poleDetection) {
+        robot.backTankDrive.respondToGamepad();
+        if (robot.backTankDrive.gamepad.right_bumper) {
+            if (poleDetection.getPole()) {
+                robot.backTankDrive.backLeftMotor.setPower(0);
+                robot.backTankDrive.backRightMotor.setPower(0);
+            } else {
+                robot.backTankDrive.backLeftMotor.setPower(-0.25);
+                robot.backTankDrive.backRightMotor.setPower(0.25);
+            }
+        }
+        if (robot.backTankDrive.gamepad.left_bumper) {
+            if (poleDetection.getPole()) {
+                robot.backTankDrive.backLeftMotor.setPower(0);
+                robot.backTankDrive.backRightMotor.setPower(0);
+            } else {
+                robot.backTankDrive.backLeftMotor.setPower(0.25);
+                robot.backTankDrive.backRightMotor.setPower(-0.25);
+                //abcdefghijklmnopqrstuvwxyz
+                //The quick brown fox jumped over the lazy dog
+                //314159265358979326538
+            }
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java
new file mode 100644
index 0000000..be2ea0f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java
@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.old.TeleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.old.Robots.NewRobot;
+
+@Disabled
+public class NewRobotTeleOp extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        NewRobot robot = new NewRobot(this, hardwareMap, telemetry, gamepad1);
+
+        robot.strafeDrive.backLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.strafeDrive.frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.strafeDrive.backRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.strafeDrive.frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        waitForStart();
+
+        while (opModeIsActive()) {
+            robot.strafeDrive.respondToGamepad();
+            telemetry.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java
new file mode 100644
index 0000000..7cf6393
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java
@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.old.TeleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
+
+@Disabled
+public class OldRobotTeleOp extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            robot.backTankDrive.respondToGamepad();
+            robot.singleJointGripperArm.respondToGamepad();
+            telemetry.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java
new file mode 100644
index 0000000..a291046
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java
@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.old.Util;
+
+public class Maths {
+    public static float clamp(float val, float min, float max) {
+        if (val < min) {
+            return min;
+        } else if (val > max) {
+            return max;
+        }
+        return val;
+    }
+
+    public static int clamp(int val, int min, int max) {
+        if (val < min) {
+            return min;
+        } else if (val > max) {
+            return max;
+        }
+        return val;
+    }
+
+    public static double clamp(double val, double min, double max) {
+        if (val < min) {
+            return min;
+        } else if (val > max) {
+            return max;
+        }
+        return val;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java
new file mode 100644
index 0000000..46227c9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.old.Util;
+
+public class Measure {
+
+    public static int dTicks(double inches) {
+        return (int)Math.round(inches * 54);
+    }
+
+    public static int sTicks(double inches) {
+        return (int)Math.round(inches * 56.25);
+    }
+}

From 190d9425840fe5f3c0c8df629f9824273a40cd90 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Fri, 3 Mar 2023 18:50:53 -0500
Subject: [PATCH 36/43] removed OG version of code

---
 .../ftc/teamcode/old/older/Auto/BOEcode.java  | 114 -----------
 .../old/older/Auto/BetterRobotAutonomous.java |  91 ---------
 .../old/older/Auto/NewBotAutoLeft.java        | 137 -------------
 .../old/older/Auto/NewBotAutoRight.java       | 147 --------------
 .../old/older/Auto/OldRobotAutonomous.java    |  49 -----
 .../older/Auto/OldRobotAutonomousOther.java   |  50 -----
 .../old/older/Auto/StrafeBotAutoLeft.java     |  55 ------
 .../teamcode/old/older/Robots/NewRobot.java   |  84 --------
 .../teamcode/old/older/Robots/OldRobot.java   |  83 --------
 .../old/older/Subsystems/BackTankDrive.java   |  79 --------
 .../old/older/Subsystems/BoeDetection.java    | 185 ------------------
 .../old/older/Subsystems/FullDetection.java   | 170 ----------------
 .../old/older/Subsystems/LinearSlide.java     | 102 ----------
 .../older/Subsystems/NewPoleDetection.java    |  84 --------
 .../old/older/Subsystems/PoleDetection.java   | 100 ----------
 .../Subsystems/SingleJointGripperArm.java     | 102 ----------
 .../old/older/Subsystems/SleeveDetection.java | 134 -------------
 .../old/older/Subsystems/StrafeDrive.java     | 176 -----------------
 .../old/older/TeleOp/CameraRobotTeleOp.java   |  80 --------
 .../old/older/TeleOp/NewRobotTeleOp.java      |  26 ---
 .../old/older/TeleOp/OldRobotTeleOp.java      |  22 ---
 .../ftc/teamcode/old/older/Util/Maths.java    |  30 ---
 .../ftc/teamcode/old/older/Util/Measure.java  |  12 --
 23 files changed, 2112 deletions(-)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java
deleted file mode 100644
index dc18033..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BOEcode.java
+++ /dev/null
@@ -1,114 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.BoeDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class BOEcode extends LinearOpMode {
-
-    BoeDetection coneDetection = new BoeDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        coneDetection = new BoeDetection();
-        camera.setPipeline(coneDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
-            @Override
-            public void onOpened() {
-                camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {
-            }
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("Cone: ", coneDetection.getPosition());
-            telemetry.update();
-        }
-
-
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(-520, 520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(375, -375, 0.5f);
-        int straight = 0;
-        boolean turningRight = true;
-        while (coneDetection.getPosition() == BoeDetection.Cone.UNDECIDED || coneDetection.getPosition() == BoeDetection.Cone.RED) {
-
-            if (turningRight) {
-                robot.backTankDrive.drive(-10, 10, 0.5f);
-                straight += 10;
-            } else {
-                robot.backTankDrive.drive(10, -10, 0.5f);
-                straight -= 10;
-            }
-            if (straight >= 500) {
-                turningRight = false;
-            } else if (straight <= -500) {
-                turningRight = true;
-            }
-        }
-
-        robot.backTankDrive.backLeftMotor.setPower(0);
-        robot.backTankDrive.backRightMotor.setPower(0);
-
-        switch (coneDetection.getPosition()) {
-            case RED:
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(-3120, -3120, 0.5f);
-                robot.singleJointGripperArm.ungrab();
-                robot.backTankDrive.drive(straight, -straight, 0.5f);
-                robot.backTankDrive.drive(-1000, 1000, 0.25f);
-                robot.backTankDrive.drive(-3000, -3000, 0.5f);
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(1000, 1000, 0.5f);
-                break;
-            case BLUE:
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(-3120, -3120, 0.5f);
-                robot.singleJointGripperArm.ungrab();
-                robot.backTankDrive.drive(straight, -straight, 0.5f);
-                robot.backTankDrive.drive(1000, -1000, 0.25f);
-                robot.backTankDrive.drive(-3000, -3000, 0.5f);
-                robot.singleJointGripperArm.grab();
-                robot.backTankDrive.drive(1000, 1000, 0.5f);
-                break;
-
-            default:
-
-                break;
-        }
-
-    }
-
-
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java
deleted file mode 100644
index 310d4d3..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/BetterRobotAutonomous.java
+++ /dev/null
@@ -1,91 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.old.Util.Maths;
-
-@Disabled
-public class BetterRobotAutonomous extends LinearOpMode {
-    private DcMotor left;
-    private DcMotor right;
-    private DcMotor arm;
-    private Servo grip1;
-    private Servo grip2;
-    private int lPos;
-    private int rPos;
-    private double gripMin = 0.45;
-    private double gripMax = 1.0;
-
-    @Override
-    public void runOpMode() {
-        left = hardwareMap.get(DcMotor.class, "b_left");
-        right = hardwareMap.get(DcMotor.class, "b_right");
-        arm = hardwareMap.get(DcMotor.class, "arm");
-        grip1 = hardwareMap.get(Servo.class, "grip1");
-        grip2 = hardwareMap.get(Servo.class, "grip2");
-
-        //Put motors in encoder mode
-        left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-
-        right.setDirection(DcMotorSimple.Direction.REVERSE);
-        //You need to have wait for start or else bad things happen
-        ungrab();
-        waitForStart();
-
-        grab();
-        move(2080, 2080, 0.50);
-        armSet(89);
-        move(520, -520, 0.50);
-        ungrab();
-        move(-520, 520, 0.50);
-        armSet(0);
-        move(2080, 2080, 0.50);
-        move(-520, 520, 0.50);
-        move(520, -520, 0.50);
-
-
-
-
-    }
-    private void move(int leftTarget, int rightTarget, double speed) {
-        lPos += leftTarget;
-        rPos += rightTarget;
-
-        left.setTargetPosition(lPos);
-        right.setTargetPosition(rPos);
-
-        left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
-        left.setPower(speed);
-        right.setPower(speed);
-
-        while(opModeIsActive() && left.isBusy() && right.isBusy()) {
-            idle();
-        }
-    }
-
-    private void armSet(int steps) {
-        arm.setTargetPosition(Maths.clamp(steps, 0, 450));
-        while (opModeIsActive() && arm.isBusy()) {
-            idle();
-        }
-    }
-    public void grab() {
-        grip1.setPosition(gripMax);
-        grip2.setPosition(1-gripMax);
-    }
-
-    public void ungrab() {
-        grip1.setPosition(gripMin);
-        grip2.setPosition(1-gripMin);
-    }
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java
deleted file mode 100644
index ab176eb..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoLeft.java
+++ /dev/null
@@ -1,137 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.SleeveDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class NewBotAutoLeft extends LinearOpMode {
-
-    SleeveDetection sleeveDetection = new SleeveDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        sleeveDetection = new SleeveDetection();
-        camera.setPipeline(sleeveDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
-        {
-            @Override
-            public void onOpened()
-            {
-                camera.startStreaming(320,240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {}
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("ROTATION: ", sleeveDetection.getPosition());
-            telemetry.update();
-        }
-
-
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(-520, 520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(375, -375, 0.5f);
-
-
-        switch(sleeveDetection.getPosition()) {
-
-            case LEFT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(3000, 3000, 0.5f);
-//                robot.backTankDrive.drive(850, -850, 0.5f);
-//                robot.singleJointGripperArm.setArm(89);
-//                robot.singleJointGripperArm.ungrab();
-//                robot.backTankDrive.drive(-3300, -3300, 0.5f);
-//                robot.backTankDrive.drive(-450, 450, 0.25f);
-                robot.backTankDrive.drive(-2200, -2200, 0.75f);
-                break;
-            case RIGHT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(150, 150, 0.5f);
-//                robot.backTankDrive.drive(-900, 900, 0.5f);
-//                robot.backTankDrive.drive(-2300, -2300, 0.5f);
-//                robot.backTankDrive.drive(900, -900, 0.5f);
-                robot.backTankDrive.drive(2450, 2450, 0.5f);
-                break;
-            default:
-                placeMiddle(robot);
-                break;
-        }
-
-    }
-    public void placeMiddle(OldRobot robot){
-        //First Cone
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(-3120, -3120, 1f);
-        robot.singleJointGripperArm.setArm(450);
-        robot.backTankDrive.drive(-500, 500, 0.6f);
-        robot.backTankDrive.drive(-350, -350, 0.25f);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(500, 500, 0.25f);
-        robot.backTankDrive.drive(500, -500, 0.6f);
-        robot.singleJointGripperArm.setArm(0);
-
-        //Second Cone
-        robot.backTankDrive.drive(-2500, -2500, 1f);
-        robot.backTankDrive.drive(450, 450, 1f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.backTankDrive.drive(1100, -1100, 0.5f);
-        robot.backTankDrive.drive(-1175, -1175, 0.5f);
-
-        robot.singleJointGripperArm.ungrab();
-        robot.singleJointGripperArm.setArm(300);
-        robot.backTankDrive.drive(200, -200, 0.6f);
-        robot.backTankDrive.drive(2000,2000, 0.5f);
-        robot.backTankDrive.drive(375, -375, 0.5f);
-        robot.backTankDrive.drive(-200,-200, 0.5f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(200,200, 0.5f);
-        robot.backTankDrive.drive(-375, 375, 0.5f);
-        robot.singleJointGripperArm.setArm(100);
-
-        //Third Cone
-        robot.backTankDrive.drive(-2000,-2000, 0.5f);
-        robot.singleJointGripperArm.ungrab();
-        robot.singleJointGripperArm.setArm(250);
-        robot.backTankDrive.drive(-1175, -1175, 0.5f);
-        robot.backTankDrive.drive(-375, 375, 0.5f);
-        robot.singleJointGripperArm.setArm(0);
-        robot.singleJointGripperArm.ungrab();
-        robot.singleJointGripperArm.setArm(100);
-        robot.backTankDrive.drive(375, -375, 0.5f);
-    }
-
-
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java
deleted file mode 100644
index b4e99d8..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/NewBotAutoRight.java
+++ /dev/null
@@ -1,147 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.SleeveDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class NewBotAutoRight extends LinearOpMode {
-
-    SleeveDetection sleeveDetection = new SleeveDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        sleeveDetection = new SleeveDetection();
-        camera.setPipeline(sleeveDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
-        {
-            @Override
-            public void onOpened()
-            {
-                camera.startStreaming(320,240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {}
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("ROTATION: ", sleeveDetection.getPosition());
-            telemetry.update();
-        }
-
-
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(-520, 520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(375, -375, 0.5f);
-        switch(sleeveDetection.getPosition()) {
-            case LEFT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(150, 150, 0.5f);
-//                robot.backTankDrive.drive(900, -900, 0.5f);
-//                robot.backTankDrive.drive(-2300, -2300, \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\0.5f);
-//                robot.backTankDrive.drive(-900, 900, 0.5f);
-                robot.backTankDrive.drive(2450, 2450, 0.5f);
-
-                break;
-            case RIGHT:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(3000, 3000, 0.5f);
-//                robot.backTankDrive.drive(-850, 850, 0.5f);
-//                robot.singleJointGripperArm.setArm(89);
-//                robot.singleJointGripperArm.ungrab();
-//                robot.backTankDrive.drive(-3300, -3300, 0.5f);
-//                robot.backTankDrive.drive(450, -450, 0.25f);
-                robot.backTankDrive.drive(-2200, -2200, 0.75f);
-                break;
-            default:
-                placeMiddle(robot);
-//                robot.backTankDrive.drive(1000, 1000, 0.5f);
-                break;
-        }
-
-    }
-
-//    public void placeMiddle2(OldRobot robot){
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-3120, -3120, 0.5f);
-//        robot.singleJointGripperArm.setArm(450);
-//        robot.backTankDrive.drive(500, -500, 0.5f);
-//        robot.backTankDrive.drive(-450, -450, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(-375, 375, 0.5f);
-//    }
-
-    public void placeMiddle(OldRobot robot){
-        //First Cone
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(-3120, -3120, 1f);
-        robot.singleJointGripperArm.setArm(450);
-        robot.backTankDrive.drive(500, -500, 0.6f);
-        robot.backTankDrive.drive(-350, -350, 0.25f);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(500, 500, 0.25f);
-        robot.backTankDrive.drive(-500, 500, 0.6f);
-        robot.singleJointGripperArm.setArm(0);
-
-        //Second Cone
-        robot.backTankDrive.drive(-2300, -2300, 1f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.backTankDrive.drive(-1100, 1100, 0.5f);
-        robot.backTankDrive.drive(-1150, -1150, 0.5f);
-
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(50, 50, 0.5f);
-        robot.singleJointGripperArm.setArm(300);
-        robot.backTankDrive.drive(-200, 200, 0.6f);
-        robot.backTankDrive.drive(2000,2000, 1f);
-        robot.backTankDrive.drive(-350, 350, 0.5f);
-        robot.singleJointGripperArm.setArm(125);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(400, -400, 0.5f);
-        robot.singleJointGripperArm.setArm(0);
-
-//        robot.backTankDrive.drive(-1900,-1900, 1f);
-//        robot.backTankDrive.drive(50, 50, 1f);
-//        robot.singleJointGripperArm.ungrab();
-//
-//        robot.singleJointGripperArm.setArm(250);
-//        robot.backTankDrive.drive(1175, 1175, 1f);
-//        robot.backTankDrive.drive(500, -500, 0.5f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.singleJointGripperArm.ungrab();
-//        robot.singleJointGripperArm.setArm(100);
-//        robot.backTankDrive.drive(-500, 500, 0.5f);
-    }
-
-
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java
deleted file mode 100644
index 06eefa0..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomous.java
+++ /dev/null
@@ -1,49 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-
-@Disabled
-public class OldRobotAutonomous extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-            //2080 ticks is 2 rotations
-        //Low Height Pole
-            //robot.singleJointGripperArm.ungrab();
-            //robot.backTankDrive.drive(-400, -400, 0.5f);
-            //robot.backTankDrive.drive(-520, 520, 0.5f);
-            //robot.singleJointGripperArm.setArm(290);
-            //robot.backTankDrive.drive(-200, -200, 0.25f);
-            //robot.singleJointGripperArm.grab();
-            //robot.backTankDrive.drive(520, 520, 0.25f);
-            //robot.singleJointGripperArm.setArm(0);
-            //robot.backTankDrive.drive(375, -375, 0.5f);
-            //robot.backTankDrive.drive(-3120, -3120, 0.5f);
-            //IMPORTANT: TO GO FORWARD PUT NEGATIVE VALUES
-        //Mid Height Pole
-            robot.singleJointGripperArm.ungrab();
-            robot.backTankDrive.drive(-3120, -3120, 0.5f);
-            robot.singleJointGripperArm.setArm(450);
-            robot.backTankDrive.drive(-520, 520, 0.5f);
-            robot.backTankDrive.drive(-400, -400, 0.25f);
-            robot.singleJointGripperArm.grab();
-            robot.backTankDrive.drive(520, 520, 0.25f);
-            robot.singleJointGripperArm.setArm(0);
-            robot.backTankDrive.drive(375, -375, 0.5f);
-            robot.backTankDrive.drive(-400, -400, 0.5f);
-    }
-
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java
deleted file mode 100644
index 96d78f0..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/OldRobotAutonomousOther.java
+++ /dev/null
@@ -1,50 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-
-@Disabled
-public class OldRobotAutonomousOther extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        //Put motors in encoder mode
-        robot.backTankDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.backTankDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.singleJointGripperArm.grab();
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-
-        //2080 ticks is 2 rotations
-//        robot.singleJointGripperArm.ungrab();
-//        robot.backTankDrive.drive(-400, -400, 0.5f);
-//        robot.backTankDrive.drive(520, -520, 0.5f);
-//        robot.singleJointGripperArm.setArm(290);
-//        robot.backTankDrive.drive(-200, -200, 0.25f);
-//        robot.singleJointGripperArm.grab();
-//        robot.backTankDrive.drive(520, 520, 0.25f);
-//        robot.singleJointGripperArm.setArm(0);
-//        robot.backTankDrive.drive(-375, 375, 0.5f);
-//        robot.backTankDrive.drive(-3120, -3120, 0.5f);
-//          IMPORTANT: TO GO FORWARD PUT NEGATIVE VALUES
-
-//Mid Height Pole
-        robot.singleJointGripperArm.ungrab();
-        robot.backTankDrive.drive(-3120, -3120, 0.5f);
-        robot.singleJointGripperArm.setArm(450);
-        robot.backTankDrive.drive(520, -520, 0.5f);
-        robot.backTankDrive.drive(-400, -400, 0.25f);
-        robot.singleJointGripperArm.grab();
-        robot.backTankDrive.drive(520, 520, 0.25f);
-        robot.singleJointGripperArm.setArm(0);
-        robot.backTankDrive.drive(-375, 375, 0.5f);
-        robot.backTankDrive.drive(-400, -400, 0.5f);
-    }
-
-
-}
-
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java
deleted file mode 100644
index c281541..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Auto/StrafeBotAutoLeft.java
+++ /dev/null
@@ -1,55 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.older.Auto;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.NewRobot;
-
-@Disabled
-public class StrafeBotAutoLeft extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        NewRobot robot = new NewRobot(this, hardwareMap, telemetry, gamepad1);
-        //Put motors in encoder mode
-        robot.strafeDrive.backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.strafeDrive.backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.strafeDrive.frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.strafeDrive.frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-//        robot.linearSlide.slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        //You need to have wait for start or else bad things happen
-        waitForStart();
-        //1350 is Approximately 25 inches moving forwards
-        //1350 is 24 inches moving sideways
-        //270 rotation is approximately 35 degrees
-
-        //Directions
-        //strafe 39.5 inches
-        //place cone
-        //strafe 12 inches
-        //rotate 180 degrees
-        //move forwards 27 inches
-
-//        robot.strafeDrive.strafe(-2221, 0.5f);
-//        robot.strafeDrive.drive(162, 162, 0.5f);
-//        robot.strafeDrive.drive(-162, -162, 0.5f);
-//        robot.strafeDrive.strafe(675, 0.5f);
-//        robot.linearSlide.grab();
-        robot.strafeDrive.strafein(-39.5, 0.5f);
-        robot.strafeDrive.strafe(200, 0.5f);
-//        robot.linearSlide.setSlide(100);
-//        robot.linearSlide.ungrab();
-//        robot.linearSlide.setSlide(0);
-        robot.strafeDrive.strafein(-12, 0.5f);
-        robot.strafeDrive.drive(-926, 926, 0.5f);
-        robot.strafeDrive.strafein(-27, 0.5f);
-
-//        robot.strafeDrive.drive(1680, 1680, 0.5f);
-//        robot.strafeDrive.strafe(1000, 0.5f);
-//        robot.strafeDrive.strafe(-1000, 0.5f);
-//        robot.strafeDrive.drive(1000, -1000, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, 0.5f);
-//        robot.strafeDrive.drive(-520,-520, -0.5f);
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java
deleted file mode 100644
index 3384481..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/NewRobot.java
+++ /dev/null
@@ -1,84 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Robots;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.Subsystems.StrafeDrive;
-import org.firstinspires.ftc.teamcode.old.Subsystems.LinearSlide;
-
-public class NewRobot {
-    private final HardwareMap hardwareMap;
-    private final Telemetry telemetry;
-
-    // Hardware Configuration Names
-    public static final String FRONT_LEFT_MOTOR_NAME = "motor_front_left";
-    public static final String FRONT_RIGHT_MOTOR_NAME = "motor_front_right";
-    public static final String BACK_LEFT_MOTOR_NAME = "motor_back_left";
-    public static final String BACK_RIGHT_MOTOR_NAME = "motor_back_right";
-    public static final String LINEAR_SLIDE_MOTOR_NAME = "motor_slide";
-    public static final String GRIPPER1_SERVO_NAME = "grip1";
-    public static final String GRIPPER2_SERVO_NAME = "grip2";
-
-    // Subsystems
-    public final StrafeDrive strafeDrive;
-    public final LinearOpMode linearOpMode;
-    public final LinearSlide linearSlide;
-
-    // Sensors
-    public final Gamepad gamepad;
-
-    public NewRobot(LinearOpMode linearOpMode, HardwareMap hardwareMap, Telemetry telemetry, Gamepad gamepad) {
-        this.hardwareMap = hardwareMap;
-        this.telemetry = telemetry;
-        this.linearOpMode = linearOpMode;
-        this.gamepad = gamepad;
-        this.strafeDrive = initStrafeDrive();
-        this.linearSlide = initLinearSlide();
-    }
-    
-    public StrafeDrive initStrafeDrive() {
-        DcMotor frontLeftMotor = (DcMotor) hardwareMap.get(FRONT_LEFT_MOTOR_NAME);
-        DcMotor frontRightMotor = (DcMotor) hardwareMap.get(FRONT_RIGHT_MOTOR_NAME);
-        DcMotor backLeftMotor = (DcMotor) hardwareMap.get(BACK_LEFT_MOTOR_NAME);
-        DcMotor backRightMotor = (DcMotor) hardwareMap.get(BACK_RIGHT_MOTOR_NAME);
-
-        backLeftMotor.setDirection(DcMotor.Direction.REVERSE);
-        frontLeftMotor.setDirection(DcMotor.Direction.REVERSE);
-
-        return new StrafeDrive(linearOpMode,
-                telemetry,
-                frontLeftMotor,
-                frontRightMotor,
-                backLeftMotor,
-                backRightMotor,
-                gamepad
-        );
-    }
-
-    public LinearSlide initLinearSlide(){
-        DcMotor slideMotor = (DcMotor) hardwareMap.get(LINEAR_SLIDE_MOTOR_NAME);
-        slideMotor.setTargetPosition(0);
-        slideMotor.setPower(1);
-        slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        slideMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
-        Servo gripper1Servo = (Servo) hardwareMap.get(GRIPPER1_SERVO_NAME);
-        Servo gripper2Servo = (Servo) hardwareMap.get(GRIPPER2_SERVO_NAME);
-        return new LinearSlide(
-                linearOpMode,
-                telemetry,
-                slideMotor,
-                gripper1Servo,
-                gripper2Servo,
-                gamepad,
-                450,
-                0,
-                0.45,
-                1
-                );
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java
deleted file mode 100644
index e99edee..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Robots/OldRobot.java
+++ /dev/null
@@ -1,83 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.older.Robots;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.older.Subsystems.BackTankDrive;
-import org.firstinspires.ftc.teamcode.old.Subsystems.SingleJointGripperArm;
-
-public class OldRobot {
-    private final HardwareMap hardwareMap;
-    private final Telemetry telemetry;
-
-    // Hardware Configuration Names
-    public static final String BACK_LEFT_MOTOR_NAME = "b_left";
-    public static final String BACK_RIGHT_MOTOR_NAME = "b_right";
-    public static final String ARM_MOTOR_NAME = "arm";
-    public static final String GRIPPER1_SERVO_NAME = "grip1";
-    public static final String GRIPPER2_SERVO_NAME = "grip2";
-
-    // Subsystems
-    public final BackTankDrive backTankDrive;
-    public final SingleJointGripperArm singleJointGripperArm;
-    public final LinearOpMode linearOpMode;
-
-    // Sensors
-    public final Gamepad gamepad;
-
-    public OldRobot(LinearOpMode linearOpMode, HardwareMap hardwareMap, Telemetry telemetry, Gamepad gamepad) {
-        this.linearOpMode = linearOpMode;
-        this.hardwareMap = hardwareMap;
-        this.telemetry = telemetry;
-        this.gamepad = gamepad;
-        this.backTankDrive = initBackTankDrive();
-        this.singleJointGripperArm = initSingleJointGripperArm();
-    }
-
-    public BackTankDrive initBackTankDrive() {
-        DcMotor backLeftMotor = (DcMotor) hardwareMap.get(BACK_LEFT_MOTOR_NAME);
-        DcMotor backRightMotor = (DcMotor) hardwareMap.get(BACK_RIGHT_MOTOR_NAME);
-
-        backRightMotor.setDirection(DcMotor.Direction.REVERSE);
-
-        return new BackTankDrive(
-                linearOpMode,
-                telemetry,
-                backLeftMotor,
-                backRightMotor,
-                gamepad,
-                1,
-                5000, // TODO: get what this value is in class
-                0.1 // TODO: measure in class what this value is
-        );
-    }
-
-    public SingleJointGripperArm initSingleJointGripperArm() {
-        DcMotor armMotor = (DcMotor) hardwareMap.get(ARM_MOTOR_NAME);
-
-        armMotor.setTargetPosition(0);
-        armMotor.setPower(1);
-        armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
-        Servo gripper1Servo = (Servo) hardwareMap.get(GRIPPER1_SERVO_NAME);
-        Servo gripper2Servo = (Servo) hardwareMap.get(GRIPPER2_SERVO_NAME);
-
-        return new SingleJointGripperArm(
-                linearOpMode,
-                telemetry,
-                armMotor,
-                gripper1Servo,
-                gripper2Servo,
-                gamepad,
-                0.45,
-                1,
-                0,
-                450
-        );
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java
deleted file mode 100644
index 3f30df0..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BackTankDrive.java
+++ /dev/null
@@ -1,79 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.older.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-public class BackTankDrive {
-    private final Telemetry telemetry;
-    public final DcMotor backLeftMotor;
-    public final DcMotor backRightMotor;
-    public final Gamepad gamepad;
-    public final int turnSensitivity;
-    public final int encoderSteps; // TODO: find out what this number is
-    public final double wheelRadius; // in meters
-    public int leftPos = 0;
-    public int rightPos = 0;
-    private LinearOpMode opMode;
-
-    public BackTankDrive(LinearOpMode opmode, Telemetry telemetry, DcMotor backLeftMotor, DcMotor backRightMotor, Gamepad gamepad, int turnSensitivity, int encoderSteps, double wheelRadius) {
-        this.opMode = opmode;
-        this.telemetry = telemetry;
-        this.backLeftMotor = backLeftMotor;
-        this.backRightMotor = backRightMotor;
-        this.gamepad = gamepad;
-        this.turnSensitivity = turnSensitivity;
-        this.encoderSteps = encoderSteps;
-        this.wheelRadius = wheelRadius;
-    }
-
-    public void respondToGamepad() {
-        if (gamepad.right_stick_x < 0.01 && gamepad.right_stick_x > -0.01 && (gamepad.left_stick_y > 0.01 || gamepad.left_stick_y < -0.01)) {
-            // Left-stick control all, going straight
-            backLeftMotor.setPower(gamepad.left_stick_y / 1.5);
-            backRightMotor.setPower(gamepad.left_stick_y / 1.5);
-        } else if (gamepad.right_stick_x < -0.01 && (gamepad.left_stick_y > 0.05 || gamepad.left_stick_y < -0.01)) {
-            // Turning left while moving
-            backLeftMotor.setPower((gamepad.left_stick_y / turnSensitivity) / 2);
-            backRightMotor.setPower(gamepad.left_stick_y / 2);
-        } else if (gamepad.right_stick_x > 0.01 && (gamepad.left_stick_y > 0.05 || gamepad.left_stick_y < -0.01)) {
-            // Turning right wile moving forward
-            backLeftMotor.setPower(gamepad.left_stick_y / 2);
-            backRightMotor.setPower((gamepad.left_stick_y / turnSensitivity) / 2);
-        } else if (gamepad.right_stick_x < -0.01 && (gamepad.left_stick_y > -0.05 || gamepad.left_stick_y < 0.01)) {
-            // Turning left without going forward
-            backRightMotor.setPower(gamepad.right_stick_x / 2);
-            backLeftMotor.setPower(-(gamepad.right_stick_x / 2));
-        } else if (gamepad.right_stick_x > 0.01 && (gamepad.left_stick_y > -0.05 || gamepad.left_stick_y < 0.01)) {
-            // Turning right without moving forward
-            backLeftMotor.setPower(-(gamepad.right_stick_x / 2));
-            backRightMotor.setPower(gamepad.right_stick_x / 2);
-        } else if (gamepad.right_stick_x == 0 && gamepad.left_stick_y == 0) {
-            backLeftMotor.setPower(0);
-            backRightMotor.setPower(0);
-        } else if (gamepad.right_bumper || gamepad.left_bumper) {
-            //covered in gamepadPlusCamera in CameraRobotTeleOp class
-        } else {
-            backLeftMotor.setPower(0);
-            backRightMotor.setPower(0);
-        }
-    }
-
-
-    public void drive(int leftmove, int rightmove, float speed) {
-        leftPos += leftmove;
-        rightPos += rightmove;
-        backLeftMotor.setTargetPosition(leftPos);
-        backRightMotor.setTargetPosition(rightPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-
-        while (opMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy()) {
-            opMode.idle();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java
deleted file mode 100644
index 13ef60d..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/BoeDetection.java
+++ /dev/null
@@ -1,185 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.older.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class BoeDetection extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum Cone {
-        RED,
-        BLUE,
-        UNDECIDED
-    }
-
-    public enum Distance {
-        CLOSE,
-        MID,
-        FAR,
-        UNSEEN
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static final int X = 55;
-    private static final int Y = 120;
-    private static final int STEP = 10;
-    private static final int WIDTH = 25;
-    private static final int HEIGHT = 15;
-    private static final Point FAR_TOPLEFT_ANCHOR_POINT = new Point(X, Y);
-    private static final Point CLOSE_TOPLEFT_ANCHOR_POINT = new Point(X - (2 * STEP), Y - (2 * STEP));
-
-    // Width and height for the bounding box
-    public static int FAR_REGION_WIDTH = WIDTH;
-    public static int FAR_REGION_HEIGHT = HEIGHT;
-    public static int MID_REGION_WIDTH = WIDTH + STEP + STEP;
-    public static int MID_REGION_HEIGHT = HEIGHT + STEP + STEP;
-    public static int CLOSE_REGION_WIDTH = WIDTH + STEP + STEP + STEP + STEP;
-    public static int CLOSE_REGION_HEIGHT = HEIGHT + STEP + STEP + STEP + STEP;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_red_bounds = new Scalar(100, 0, 0, 255),
-            upper_red_bounds = new Scalar(255, 150, 150, 255),
-            lower_blue_bounds = new Scalar(0, 0, 100, 255),
-            upper_blue_bounds = new Scalar(150, 150, 255, 255);
-
-    // Color definitions
-    private final Scalar
-            RED = new Scalar(255, 0, 0),
-            BLUE = new Scalar(0, 0, 255),
-            BLACK = new Scalar(0, 0, 0);
-
-    // Percent and mat definitions
-    private double redPercentC, bluePercentC, redPercentM, bluePercentM, redPercentF, bluePercentF;
-    private final Mat redMatC = new Mat();
-    private final Mat blueMatC = new Mat();
-    private Mat redMatF = new Mat();
-    private Mat blueMatF = new Mat();
-    private Mat redMatM = new Mat();
-    private Mat blueMatM = new Mat();
-    private Mat blurredMatClose = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point((2 * STEP), (2 * STEP));
-    Point sleeve_pointB = new Point(
-            (2 * STEP) + FAR_REGION_WIDTH,
-            (2 * STEP) + FAR_REGION_HEIGHT);
-
-    Point sleeve_pointC = new Point(STEP, STEP);
-    Point sleeve_pointD = new Point(
-            STEP + MID_REGION_WIDTH,
-            STEP + MID_REGION_HEIGHT);
-
-    Point sleeve_pointE = new Point(
-            CLOSE_TOPLEFT_ANCHOR_POINT.x,
-            CLOSE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointF = new Point(
-            CLOSE_TOPLEFT_ANCHOR_POINT.x + CLOSE_REGION_WIDTH,
-            CLOSE_TOPLEFT_ANCHOR_POINT.y + CLOSE_REGION_HEIGHT);
-
-    // Running variable storing the parking position
-    private volatile Cone cone = Cone.UNDECIDED;
-    private volatile Distance distance = Distance.UNSEEN;
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMatClose, new Size(5, 5));
-        blurredMatClose = blurredMatClose.submat(new Rect(sleeve_pointE, sleeve_pointF));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMatClose, blurredMatClose, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMatClose, lower_red_bounds, upper_red_bounds, redMatC);
-        Core.inRange(blurredMatClose, lower_blue_bounds, upper_blue_bounds, blueMatC);
-
-        redMatM = redMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
-        blueMatM = blueMatC.submat(new Rect(sleeve_pointC, sleeve_pointD));
-
-        redMatF = redMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
-        blueMatF = blueMatC.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-
-        // Gets color specific values
-        redPercentC = Core.countNonZero(redMatC);
-        bluePercentC = Core.countNonZero(blueMatC);
-
-        redPercentM = Core.countNonZero(redMatM);
-        bluePercentM = Core.countNonZero(blueMatM);
-
-        redPercentF = Core.countNonZero(redMatF);
-        bluePercentF = Core.countNonZero(blueMatF);
-
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(redPercentF, bluePercentF);
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == redPercentF) {
-            if (redPercentC >= 150) {
-                cone = Cone.RED;
-                distance = Distance.CLOSE;
-            } else if (redPercentM >= 100) {
-                cone = Cone.RED;
-                distance = Distance.MID;
-            } else if (redPercentF >= 50) {
-                cone = Cone.RED;
-                distance = Distance.FAR;
-            } else {
-                cone = Cone.UNDECIDED;
-                distance = Distance.UNSEEN;
-            }
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    RED,
-                    2
-            );
-        } else if (maxPercent == bluePercentC && bluePercentC >= 50) {
-            cone = Cone.BLUE;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    BLUE,
-                    2
-            );
-        } else {
-            cone = Cone.UNDECIDED;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    BLACK,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        redMatC.release();
-        blueMatC.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public Cone getPosition() {
-        return cone;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java
deleted file mode 100644
index 7c560e8..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/FullDetection.java
+++ /dev/null
@@ -1,170 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class FullDetection extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum ParkingPosition {
-        LEFT,
-        CENTER,
-        RIGHT
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(55, 120);
-    private static Point POLE_TOPLEFT_ANCHOR_POINT = new Point(75, 100);
-
-    // Width and height for the bounding box
-    public static int SLEEVE_REGION_WIDTH = 25;
-    public static int SLEEVE_REGION_HEIGHT = 15;
-    public static int POLE_REGION_WIDTH = 25;
-    public static int POLE_REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds  = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds  = new Scalar(255, 255, 150, 255),
-            lower_cyan_bounds    = new Scalar(0, 120, 120, 255),
-            upper_cyan_bounds    = new Scalar(150, 255, 255, 255),
-            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
-            upper_magenta_bounds = new Scalar(255, 170, 255, 255),
-            lower_pole_bounds = new Scalar(150, 150, 0, 255),
-            upper_pole_bounds = new Scalar(255, 255, 150, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW  = new Scalar(255, 255, 0),
-            CYAN    = new Scalar(0, 255, 255),
-            MAGENTA = new Scalar(255, 0, 255);
-
-    // Percent and mat definitions
-    private double yelPercentS, cyaPercentS, magPercentS, yelPercentP;
-    private Mat yelMatS = new Mat(), cyaMatS = new Mat(), magMatS = new Mat(), yelMatP = new Mat(), blurredMat = new Mat(), sleeveMat = new Mat(), poleMat = new Mat(), kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + SLEEVE_REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + SLEEVE_REGION_HEIGHT);
-    Point pole_pointA = new Point(
-            POLE_TOPLEFT_ANCHOR_POINT.x,
-            POLE_TOPLEFT_ANCHOR_POINT.y);
-    Point pole_pointB = new Point(
-            POLE_TOPLEFT_ANCHOR_POINT.x + POLE_REGION_WIDTH,
-            POLE_TOPLEFT_ANCHOR_POINT.y + POLE_REGION_HEIGHT);
-
-    // Running variable storing the parking position and pole detection
-    private volatile ParkingPosition position = ParkingPosition.LEFT;
-    private volatile boolean isPole = false;
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        sleeveMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-        poleMat = blurredMat.submat(new Rect(pole_pointA, pole_pointB));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(sleeveMat, sleeveMat, Imgproc.MORPH_CLOSE, kernel);
-        Imgproc.morphologyEx(poleMat, poleMat, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(sleeveMat, lower_yellow_bounds, upper_yellow_bounds, yelMatS);
-        Core.inRange(sleeveMat, lower_cyan_bounds, upper_cyan_bounds, cyaMatS);
-        Core.inRange(sleeveMat, lower_magenta_bounds, upper_magenta_bounds, magMatS);
-        Core.inRange(poleMat, lower_pole_bounds, upper_pole_bounds, yelMatP);
-
-        // Gets color specific values
-        yelPercentS = Core.countNonZero(yelMatS);
-        cyaPercentS = Core.countNonZero(cyaMatS);
-        magPercentS = Core.countNonZero(magMatS);
-        yelPercentP = Core.countNonZero(yelMatP);
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(yelPercentS, Math.max(cyaPercentS, magPercentS));
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == yelPercentS) {
-            position = ParkingPosition.LEFT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else if (maxPercent == cyaPercentS) {
-            position = ParkingPosition.CENTER;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        } else if (maxPercent == magPercentS) {
-            position = ParkingPosition.RIGHT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    MAGENTA,
-                    2
-            );
-        }
-        if (yelPercentP >= 60) {
-            isPole = true;
-            Imgproc.rectangle(
-                    input,
-                    pole_pointA,
-                    pole_pointB,
-                    YELLOW,
-                    2
-            );
-        } else {
-            isPole = false;
-            Imgproc.rectangle(
-                    input,
-                    pole_pointA,
-                    pole_pointB,
-                    CYAN,
-                    2
-            );
-        }
-        // Memory cleanup
-        blurredMat.release();
-        yelMatS.release();
-        cyaMatS.release();
-        magMatS.release();
-        yelMatP.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public ParkingPosition getPosition() {
-        return position;
-    }
-    public boolean getPole() {
-        return isPole;
-    }
-    public double getPercent() {
-        return yelPercentP;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java
deleted file mode 100644
index 705e63b..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/LinearSlide.java
+++ /dev/null
@@ -1,102 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.Util.Maths;
-
-public class LinearSlide {
-    private final Telemetry telemetry;
-    public final DcMotor slideMotor;
-    public final Servo gripper1Servo;
-    public final Servo gripper2Servo;
-    public final Gamepad gamepad;
-    public final int minHeight;
-    public final int maxHeight;
-    public final double gripMin;
-    public final double gripMax;
-    private double gripPos = 1;
-    private double height = 0;
-    private int armMotorSteps =0;
-    private boolean dpadpressed = false;
-    private LinearOpMode linearOpMode;
-
-    public LinearSlide(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor slideMotor, Servo gripper1, Servo gripper2, Gamepad gamepad, int maxHeight, int minHeight, double gripMin, double gripMax){
-        this.linearOpMode = linearOpMode;
-        this.telemetry = telemetry;
-        this.slideMotor = slideMotor;
-        this.gripper1Servo = gripper1;
-        this.gripper2Servo = gripper2;
-        this.gamepad = gamepad;
-        this.minHeight = minHeight;
-        this.maxHeight = maxHeight;
-        this.gripMin = gripMin;
-        this.gripMax = gripMax;
-    }
-
-    public void respondToGamepad() {
-        slideRespondToGamepad();
-        gripRespondToGamepad();
-    }
-
-    public void gripRespondToGamepad() {
-        if (gamepad.right_trigger > 0.5) {
-            gripPos = gripMin;
-        } else if (gamepad.left_trigger > 0.5) {
-            gripPos = gripMax;
-        }
-
-        gripPos = Maths.clamp(gripPos, gripMin, gripMax);
-
-        gripper1Servo.setPosition(1 - gripPos);
-        gripper2Servo.setPosition(gripPos);
-    }
-
-    public void slideRespondToGamepad(){
-        if (gamepad.a) {
-            armMotorSteps = 0;
-        } else if (gamepad.b) {
-            armMotorSteps = 86;
-        } else if (gamepad.x) {
-            armMotorSteps = 270;
-        } else if (gamepad.y) {
-            armMotorSteps = 460;
-        }
-
-        // TODO: check if this sensitivity is right
-        if (gamepad.dpad_up) {
-            armMotorSteps += 4;
-            dpadpressed = true;
-        } else if (gamepad.dpad_down) {
-            armMotorSteps -= 4;
-            dpadpressed = true;
-        } else if (dpadpressed) {
-            armMotorSteps = slideMotor.getCurrentPosition();
-            dpadpressed = false;
-        }
-
-        armMotorSteps = Maths.clamp(armMotorSteps, minHeight, maxHeight);
-
-        slideMotor.setTargetPosition(armMotorSteps);
-    }
-
-    public void grab() {
-        gripper1Servo.setPosition(1);
-        gripper2Servo.setPosition(0);
-    }
-
-    public void ungrab() {
-        gripper1Servo.setPosition(gripMin);
-        gripper2Servo.setPosition(1-gripMin);
-    }
-
-    public void setSlide(int steps) {
-        slideMotor.setTargetPosition(Maths.clamp(steps, minHeight, maxHeight));
-        while (linearOpMode.opModeIsActive() && slideMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java
deleted file mode 100644
index a395cc2..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/NewPoleDetection.java
+++ /dev/null
@@ -1,84 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class NewPoleDetection extends OpenCvPipeline {
-
-
-    private volatile boolean isPole = false;
-
-    // TOPLEFT anchor point for the bounding box
-    private static Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(45, 60);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds  = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds  = new Scalar(255, 255, 150, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW  = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255);
-
-    // Percent and mat definitions
-    private double yelPercent;
-    private Mat yelMat = new Mat(), blurredMat = new Mat(), kernel = new Mat(), poleEdge = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        //Black and white
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
-
-
-        // Gets color specific values
-        yelPercent = Core.countNonZero(yelMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (yelPercent >= 60) {
-            isPole = true;
-        } else {
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    public boolean getPole() {
-        return isPole;
-    }
-    public double getPercent() {
-        return yelPercent;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java
deleted file mode 100644
index 088a4eb..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/PoleDetection.java
+++ /dev/null
@@ -1,100 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class PoleDetection extends OpenCvPipeline {
-    private volatile boolean isPole = false;
-
-    // TOPLEFT anchor point for the bounding box
-    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(45, 60);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds = new Scalar(255, 255, 150, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255);
-
-    // Percent and mat definitions
-    private double yelPercent;
-    private final Mat yelMat = new Mat();
-    private Mat blurredMat = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-
-        // Gets color specific values
-        yelPercent = Core.countNonZero(yelMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (yelPercent >= 60) {
-            isPole = true;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else {
-            isPole = false;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    public boolean getPole() {
-        return isPole;
-    }
-
-    public double getPercent() {
-        return yelPercent;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java
deleted file mode 100644
index 12c4b83..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SingleJointGripperArm.java
+++ /dev/null
@@ -1,102 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.old.Util.Maths;
-
-public class SingleJointGripperArm {
-    private final Telemetry telemetry;
-    public final DcMotor armMotor;
-    public final Servo gripper1Servo;
-    public final Servo gripper2Servo;
-    public final Gamepad gamepad;
-    public final double gripMin;
-    public final double gripMax;
-    public final int armMin;
-    public final int armMax;
-    private int armMotorSteps = 0;
-    private double gripPos = 1;
-    private LinearOpMode linearOpMode;
-    private boolean dpadpressed = false;
-
-    public SingleJointGripperArm(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor armMotor, Servo gripper1Servo, Servo gripper2Servo, Gamepad gamepad, double gripMin, double gripMax, int armMin, int armMax) {
-        this.linearOpMode = linearOpMode;
-        this.telemetry = telemetry;
-        this.armMotor = armMotor;
-        this.gripper1Servo = gripper1Servo;
-        this.gripper2Servo = gripper2Servo;
-        this.gamepad = gamepad;
-        this.gripMin = gripMin;
-        this.gripMax = gripMax;
-        this.armMin = armMin;
-        this.armMax = armMax;
-    }
-
-    public void respondToGamepad() {
-        armRespondToGamepad();
-        gripRespondToGamepad();
-    }
-
-    public void gripRespondToGamepad() {
-        if (gamepad.right_trigger > 0.5) {
-            gripPos = gripMin;
-        } else if (gamepad.left_trigger > 0.5) {
-            gripPos = gripMax;
-        }
-
-        gripPos = Maths.clamp(gripPos, gripMin, gripMax);
-
-        gripper1Servo.setPosition(1 - gripPos);
-        gripper2Servo.setPosition(gripPos);
-    }
-
-    public void armRespondToGamepad() {
-        if (gamepad.a) {
-            armMotorSteps = 0;
-        } else if (gamepad.b) {
-            armMotorSteps = 86;
-        } else if (gamepad.x) {
-            armMotorSteps = 270;
-        } else if (gamepad.y) {
-            armMotorSteps = 460;
-        }
-
-        // TODO: check if this sensitivity is right
-        if (gamepad.dpad_up) {
-            armMotorSteps += 4;
-            dpadpressed = true;
-        } else if (gamepad.dpad_down) {
-            armMotorSteps -= 4;
-            dpadpressed = true;
-        } else if (dpadpressed) {
-            armMotorSteps = armMotor.getCurrentPosition();
-            dpadpressed = false;
-        }
-
-        armMotorSteps = Maths.clamp(armMotorSteps, armMin, armMax);
-
-        armMotor.setTargetPosition(armMotorSteps);
-    }
-
-    public void grab() {
-        gripper1Servo.setPosition(0);
-        gripper2Servo.setPosition(1);
-    }
-
-    public void ungrab() {
-        gripper1Servo.setPosition(1-gripMin);
-        gripper2Servo.setPosition(gripMin);
-    }
-
-    public void setArm(int steps) {
-        armMotor.setTargetPosition(Maths.clamp(steps, armMin, armMax));
-        while (linearOpMode.opModeIsActive() && armMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java
deleted file mode 100644
index 379c904..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/SleeveDetection.java
+++ /dev/null
@@ -1,134 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Subsystems;
-
-import org.opencv.core.Core;
-import org.opencv.core.Mat;
-import org.opencv.core.Point;
-import org.opencv.core.Rect;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-import org.openftc.easyopencv.OpenCvPipeline;
-
-public class SleeveDetection extends OpenCvPipeline {
-    /*
-    YELLOW  = Parking Left
-    CYAN    = Parking Middle
-    MAGENTA = Parking Right
-     */
-
-    public enum ParkingPosition {
-        LEFT,
-        CENTER,
-        RIGHT
-    }
-
-    // TOPLEFT anchor point for the bounding box
-    private static final Point SLEEVE_TOPLEFT_ANCHOR_POINT = new Point(55, 120);
-
-    // Width and height for the bounding box
-    public static int REGION_WIDTH = 25;
-    public static int REGION_HEIGHT = 15;
-
-    // Lower and upper boundaries for colors
-    private static final Scalar
-            lower_yellow_bounds = new Scalar(150, 150, 0, 255),
-            upper_yellow_bounds = new Scalar(255, 255, 150, 255),
-            lower_cyan_bounds = new Scalar(0, 120, 120, 255),
-            upper_cyan_bounds = new Scalar(150, 255, 255, 255),
-            lower_magenta_bounds = new Scalar(120, 0, 120, 255),
-            upper_magenta_bounds = new Scalar(255, 170, 255, 255);
-
-    // Color definitions
-    private final Scalar
-            YELLOW = new Scalar(255, 255, 0),
-            CYAN = new Scalar(0, 255, 255),
-            MAGENTA = new Scalar(255, 0, 255);
-
-    // Percent and mat definitions
-    private double yelPercent, cyaPercent, magPercent;
-    private final Mat yelMat = new Mat();
-    private final Mat cyaMat = new Mat();
-    private final Mat magMat = new Mat();
-    private Mat blurredMat = new Mat();
-    private Mat kernel = new Mat();
-
-    // Anchor point definitions
-    Point sleeve_pointA = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y);
-    Point sleeve_pointB = new Point(
-            SLEEVE_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH,
-            SLEEVE_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT);
-
-    // Running variable storing the parking position
-    private volatile ParkingPosition position = ParkingPosition.LEFT;
-
-    @Override
-    public Mat processFrame(Mat input) {
-        // Noise reduction
-        Imgproc.blur(input, blurredMat, new Size(5, 5));
-        blurredMat = blurredMat.submat(new Rect(sleeve_pointA, sleeve_pointB));
-
-        // Apply Morphology
-        kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3));
-        Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel);
-
-        // Gets channels from given source mat
-        Core.inRange(blurredMat, lower_yellow_bounds, upper_yellow_bounds, yelMat);
-        Core.inRange(blurredMat, lower_cyan_bounds, upper_cyan_bounds, cyaMat);
-        Core.inRange(blurredMat, lower_magenta_bounds, upper_magenta_bounds, magMat);
-
-        // Gets color specific values
-        yelPercent = Core.countNonZero(yelMat);
-        cyaPercent = Core.countNonZero(cyaMat);
-        magPercent = Core.countNonZero(magMat);
-
-        // Calculates the highest amount of pixels being covered on each side
-        double maxPercent = Math.max(yelPercent, Math.max(cyaPercent, magPercent));
-
-        // Checks all percentages, will highlight bounding box in camera preview
-        // based on what color is being detected
-        if (maxPercent == yelPercent) {
-            position = ParkingPosition.LEFT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    YELLOW,
-                    2
-            );
-        } else if (maxPercent == cyaPercent) {
-            position = ParkingPosition.CENTER;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    CYAN,
-                    2
-            );
-        } else if (maxPercent == magPercent) {
-            position = ParkingPosition.RIGHT;
-            Imgproc.rectangle(
-                    input,
-                    sleeve_pointA,
-                    sleeve_pointB,
-                    MAGENTA,
-                    2
-            );
-        }
-
-        // Memory cleanup
-        blurredMat.release();
-        yelMat.release();
-        cyaMat.release();
-        magMat.release();
-        kernel.release();
-
-        return input;
-    }
-
-    // Returns an enum being the current position where the robot will park
-    public ParkingPosition getPosition() {
-        return position;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java
deleted file mode 100644
index c4b6140..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Subsystems/StrafeDrive.java
+++ /dev/null
@@ -1,176 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.older.Subsystems;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import org.firstinspires.ftc.teamcode.old.Util.Measure;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-public class StrafeDrive {
-    private final Telemetry telemetry;
-    public final DcMotor frontLeftMotor;
-    public final DcMotor frontRightMotor;
-    public final DcMotor backLeftMotor;
-    public final DcMotor backRightMotor;
-    public final LinearOpMode linearOpMode;
-    public final Gamepad gamepad;
-    public int flPos = 0;
-    public int frPos = 0;
-    public int blPos = 0;
-    public int brPos = 0;
-
-    public StrafeDrive(LinearOpMode linearOpMode, Telemetry telemetry, DcMotor frontLeftMotor, DcMotor frontRightMotor, DcMotor backLeftMotor, DcMotor backRightMotor, Gamepad gamepad) {
-        this.telemetry = telemetry;
-        this.linearOpMode = linearOpMode;
-        this.frontLeftMotor = frontLeftMotor;
-        this.frontRightMotor = frontRightMotor;
-        this.backLeftMotor = backLeftMotor;
-        this.backRightMotor = backRightMotor;
-        this.gamepad = gamepad;
-    }
-
-
-    public void respondToGamepad() {
-
-        double forward = -gamepad.left_stick_y;
-        double strafe = gamepad.left_stick_x;
-        double rotate = gamepad.right_stick_x;
-        if (Math.abs(-gamepad.left_stick_y) < 0.01) {
-            forward = 0;
-        }
-
-        if (Math.abs(gamepad.left_stick_x) < 0.01) {
-            strafe = 0;
-        }
-
-        if (Math.abs(gamepad.right_stick_x) < 0.01) {
-            rotate = 0;
-        }
-        frontLeftMotor.setPower(forward + strafe + rotate);
-        backLeftMotor.setPower(forward - strafe + rotate);
-        frontRightMotor.setPower(forward + strafe - rotate);
-        backRightMotor.setPower(forward - strafe - rotate);
-    }
-
-    public void drive(int leftMove, int rightMove, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = leftMove;
-        frPos = rightMove;
-        blPos = leftMove;
-        brPos = rightMove;
-
-
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-    public void drivein(double leftinches, int rightinches, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = Measure.sTicks(leftinches);
-        frPos = Measure.sTicks(rightinches);
-        blPos = Measure.sTicks(leftinches);
-        brPos = Measure.sTicks(rightinches);
-
-
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-    public void strafe(int move, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = move;
-        frPos = move;
-        blPos = move * -1;
-        brPos = move * -1;
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-    public void strafein(double inches, float speed) {
-
-        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        flPos = Measure.sTicks(inches);
-        frPos = Measure.sTicks(inches);
-        blPos = Measure.sTicks(inches) * -1;
-        brPos = Measure.sTicks(inches) * -1;
-        backLeftMotor.setTargetPosition(blPos);
-        frontRightMotor.setTargetPosition(frPos);
-        backRightMotor.setTargetPosition(brPos);
-        frontLeftMotor.setTargetPosition(flPos);
-        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        backLeftMotor.setPower(speed);
-        backRightMotor.setPower(speed);
-        frontLeftMotor.setPower(speed);
-        frontRightMotor.setPower(speed);
-
-        while (linearOpMode.opModeIsActive() && backLeftMotor.isBusy() && backRightMotor.isBusy() && frontLeftMotor.isBusy() && frontRightMotor.isBusy()) {
-            linearOpMode.idle();
-        }
-    }
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java
deleted file mode 100644
index 20f8fc6..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/CameraRobotTeleOp.java
+++ /dev/null
@@ -1,80 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.older.TeleOp;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.teamcode.old.older.Robots.OldRobot;
-import org.firstinspires.ftc.teamcode.old.Subsystems.PoleDetection;
-import org.openftc.easyopencv.OpenCvCamera;
-import org.openftc.easyopencv.OpenCvCameraFactory;
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-@Disabled
-public class CameraRobotTeleOp extends LinearOpMode {
-
-    PoleDetection poleDetection = new PoleDetection();
-    OpenCvCamera camera;
-    String webcamName = "Webcam 1";
-
-    @Override
-    public void runOpMode() {
-        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId);
-        poleDetection = new PoleDetection();
-        camera.setPipeline(poleDetection);
-
-        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
-            @Override
-            public void onOpened() {
-                camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT);
-            }
-
-            @Override
-            public void onError(int errorCode) {
-            }
-        });
-
-        while (!isStarted()) {
-            telemetry.addData("Is there a pole?: ", poleDetection.getPole());
-            telemetry.addData("Percent Yellow: ", poleDetection.getPercent());
-            telemetry.update();
-        }
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        waitForStart();
-
-        while (opModeIsActive()) {
-            gamepadPlusCamera(robot, poleDetection);
-            robot.singleJointGripperArm.respondToGamepad();
-            telemetry.addData("Is there a pole?: ", poleDetection.getPole());
-            telemetry.addData("Percent Yellow: ", poleDetection.getPercent());
-            telemetry.update();
-        }
-    }
-
-    private void gamepadPlusCamera(OldRobot robot, PoleDetection poleDetection) {
-        robot.backTankDrive.respondToGamepad();
-        if (robot.backTankDrive.gamepad.right_bumper) {
-            if (poleDetection.getPole()) {
-                robot.backTankDrive.backLeftMotor.setPower(0);
-                robot.backTankDrive.backRightMotor.setPower(0);
-            } else {
-                robot.backTankDrive.backLeftMotor.setPower(-0.25);
-                robot.backTankDrive.backRightMotor.setPower(0.25);
-            }
-        }
-        if (robot.backTankDrive.gamepad.left_bumper) {
-            if (poleDetection.getPole()) {
-                robot.backTankDrive.backLeftMotor.setPower(0);
-                robot.backTankDrive.backRightMotor.setPower(0);
-            } else {
-                robot.backTankDrive.backLeftMotor.setPower(0.25);
-                robot.backTankDrive.backRightMotor.setPower(-0.25);
-                //abcdefghijklmnopqrstuvwxyz
-                //The quick brown fox jumped over the lazy dog
-                //314159265358979326538
-            }
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java
deleted file mode 100644
index be2ea0f..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/NewRobotTeleOp.java
+++ /dev/null
@@ -1,26 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.TeleOp;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-
-import org.firstinspires.ftc.teamcode.old.Robots.NewRobot;
-
-@Disabled
-public class NewRobotTeleOp extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        NewRobot robot = new NewRobot(this, hardwareMap, telemetry, gamepad1);
-
-        robot.strafeDrive.backLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.strafeDrive.frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.strafeDrive.backRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.strafeDrive.frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        waitForStart();
-
-        while (opModeIsActive()) {
-            robot.strafeDrive.respondToGamepad();
-            telemetry.update();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java
deleted file mode 100644
index 7cf6393..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/TeleOp/OldRobotTeleOp.java
+++ /dev/null
@@ -1,22 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.TeleOp;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.old.Robots.OldRobot;
-
-@Disabled
-public class OldRobotTeleOp extends LinearOpMode {
-    @Override
-    public void runOpMode() {
-        OldRobot robot = new OldRobot(this, hardwareMap, telemetry, gamepad1);
-
-        waitForStart();
-
-        while (opModeIsActive()) {
-            robot.backTankDrive.respondToGamepad();
-            robot.singleJointGripperArm.respondToGamepad();
-            telemetry.update();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java
deleted file mode 100644
index a291046..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Maths.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Util;
-
-public class Maths {
-    public static float clamp(float val, float min, float max) {
-        if (val < min) {
-            return min;
-        } else if (val > max) {
-            return max;
-        }
-        return val;
-    }
-
-    public static int clamp(int val, int min, int max) {
-        if (val < min) {
-            return min;
-        } else if (val > max) {
-            return max;
-        }
-        return val;
-    }
-
-    public static double clamp(double val, double min, double max) {
-        if (val < min) {
-            return min;
-        } else if (val > max) {
-            return max;
-        }
-        return val;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java
deleted file mode 100644
index 46227c9..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/older/Util/Measure.java
+++ /dev/null
@@ -1,12 +0,0 @@
-package org.firstinspires.ftc.teamcode.old.Util;
-
-public class Measure {
-
-    public static int dTicks(double inches) {
-        return (int)Math.round(inches * 54);
-    }
-
-    public static int sTicks(double inches) {
-        return (int)Math.round(inches * 56.25);
-    }
-}

From 8c84039333a7d09366d3d8149410303406e281c2 Mon Sep 17 00:00:00 2001
From: Broyojo <dha@xoba.com>
Date: Sat, 4 Mar 2023 15:36:02 -0500
Subject: [PATCH 37/43] re-tuned with linear actuator and feedforward

---
 .../ftc/teamcode/auto/AutoFunny.java          | 11 +++----
 .../subsystem/drive/DriveConstants.java       | 29 ++++++++++---------
 .../subsystem/drive/SampleMecanumDrive.java   |  6 ++--
 3 files changed, 22 insertions(+), 24 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
index ec789c4..13eb2da 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoFunny.java
@@ -34,14 +34,11 @@ public void runOpMode() throws InterruptedException {
 //                .splineToConstantHeading(new Vector2d(26.97, -56.94), Math.toRadians(199.98))
 //                .splineToConstantHeading(new Vector2d(-37.01, -12.88), Math.toRadians(175.60))
 //                .build();
-        TrajectorySequence traj = drive.trajectorySequenceBuilder(new Pose2d(-37.34, -65.52, Math.toRadians(88.15)))
-                .splineTo(new Vector2d(-6.40, -15.31), Math.toRadians(2.23))
-                .splineToConstantHeading(new Vector2d(26.97, -56.94), Math.toRadians(199.98))
-                .splineToConstantHeading(new Vector2d(-37.01, -12.88), Math.toRadians(175.60))
-                .splineToConstantHeading(new Vector2d(-58.39, -18.71), Math.toRadians(-84.62))
-                .splineToConstantHeading(new Vector2d(23.73, -16.44), Math.toRadians(2.79))
-                .splineTo(new Vector2d(-37.34, -65.68), Math.toRadians(-90.00))
+        TrajectorySequence traj = drive.trajectorySequenceBuilder(new Pose2d(-32.48, -67.46, Math.toRadians(90.00)))
+                .splineToConstantHeading(new Vector2d(-13.20, -52.56), Math.toRadians(88.52))
+                .splineToConstantHeading(new Vector2d(-20.65, -4.13), Math.toRadians(128.21))
                 .build();
+        drive.setPoseEstimate(traj.start());
 
         drive.setPoseEstimate(traj.start());
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/DriveConstants.java
index fbde9de..9f754cb 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/DriveConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/DriveConstants.java
@@ -33,8 +33,9 @@ public class DriveConstants {
      * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
      * from DriveVelocityPIDTuner.
      */
-    public static final boolean RUN_USING_ENCODER = true;
-    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(20, 0, 5, 12.627677180197198); // f = getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); // p=30, d=12
+    public static final boolean RUN_USING_ENCODER = false;
+    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
+            getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
 
     /*
      * These are physical constants that can be determined from your robot (including the track
@@ -45,8 +46,8 @@ public class DriveConstants {
      * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
      */
     public static double WHEEL_RADIUS = 1.4763; // in
-    public static double GEAR_RATIO = 0.05584806; // 0.05291; // output (wheel) speed / input (motor) speed
-    public static double TRACK_WIDTH = 9.58; // 10.56; // 10.81; // 13.75; // in
+    public static double GEAR_RATIO = 0.05584806; // output (wheel) speed / input (motor) speed
+    public static double TRACK_WIDTH = 10.17; // 9.58; // in
 
     /*
      * These are the feedforward parameters used to model the drive motor behavior. If you are using
@@ -54,9 +55,9 @@ public class DriveConstants {
      * motor encoders or have elected not to use them for velocity control, these values should be
      * empirically tuned.
      */
-    public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
-    public static double kA = 0;
-    public static double kStatic = 0;
+    public static double kV = 0.01749; // 1.0 / rpmToVelocity(MAX_RPM);
+    public static double kA = 0.002; // 0;
+    public static double kStatic = 0.07870; // 0;
 
     /*
      * These values are used to generate the trajectories for you robot. To ensure proper operation,
@@ -69,27 +70,27 @@ public class DriveConstants {
      * Note from LearnRoadRunner.com:
      * The velocity and acceleration constraints were calculated based on the following equation:
      * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
-     * Resulting in 41.716818064308676 in/s.
+     * Resulting in 44.03332750452835 in/s.
      * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
      * This is capped at 85% because there are a number of variables that will prevent your bot from actually
      * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
      * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
-     * max velocity. The theoretically maximum velocity is 49.07860948742197 in/s.
+     * max velocity. The theoretically maximum velocity is 51.80391471120983 in/s.
      * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
      * affected if it is aiming for a velocity not actually possible.
      *
      * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
      * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
-     * to degrade. As of now, it simply mirrors the velocity, resulting in 41.716818064308676 in/s/s
+     * to degrade. As of now, it simply mirrors the velocity, resulting in 44.03332750452835 in/s/s
      *
      * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
      * You are free to raise this on your own if you would like. It is best determined through experimentation.
 
      */
-    public static double MAX_VEL = 38.56039361602114; // 41.716818064308676;
-    public static double MAX_ACCEL = 41.716818064308676;
-    public static double MAX_ANG_VEL = Math.toRadians(142.4500100542844); // 173.83255343999997
-    public static double MAX_ANG_ACCEL = Math.toRadians(173.83255343999997);
+    public static double MAX_VEL = 44.03332750452835;
+    public static double MAX_ACCEL = 44.03332750452835;
+    public static double MAX_ANG_VEL = Math.toRadians(136.4000251969352); // 263.3532175288935
+    public static double MAX_ANG_ACCEL = Math.toRadians(263.3532175288935);
 
 
     public static double encoderTicksToInches(double ticks) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
index 3e76e8a..426c8db 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/drive/SampleMecanumDrive.java
@@ -54,10 +54,10 @@
  */
 @Config
 public class SampleMecanumDrive extends MecanumDrive {
-    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 0);
-    public static PIDCoefficients HEADING_PID = new PIDCoefficients(3, 0, 0);
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 1); // 3, 0, 0
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); // 3, 0, 0
 
-    public static double LATERAL_MULTIPLIER = 1.0813788830983113; // 1
+    public static double LATERAL_MULTIPLIER = 1; // 1.0813788830983113
 
     public static double VX_WEIGHT = 1;
     public static double VY_WEIGHT = 1;

From 895e873a0de278f6ea7b4e8e50e4e56bbd04decf Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 9 Mar 2023 15:33:22 -0500
Subject: [PATCH 38/43] Working 3 Cone Auto

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 156 +++++++++++-------
 1 file changed, 93 insertions(+), 63 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index 925bfb6..bf0ef12 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -38,45 +38,15 @@ public void runOpMode() throws InterruptedException {
         Webcam cam = new Webcam(hardwareMap, sleeveDetection);
         ColorSensor colorSensor = new ColorSensor(hardwareMap);
 
-        int[] heights = {480, 360, 180, 0, 0};
-        int time = 0;
-
         SleeveDetectionNewOld.ParkingPosition position;
-        TrajectorySequence traj1 = drive.trajectorySequenceBuilder(new Pose2d())
-                .addDisplacementMarker(slide::grab)
-                .addDisplacementMarker(() -> slide.setSlide(60))
-                .splineToLinearHeading(new Pose2d(10, -22, Math.toRadians(55)), Math.toRadians(0))
-                .splineToLinearHeading(new Pose2d(58, -21, Math.toRadians(55)), Math.toRadians(0))
-                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .forward(4.5)
-                .waitSeconds(1.5)
-                .addDisplacementMarker(slide::ungrab)
-                .waitSeconds(1.5)
-                .back(4.5)
-                .addDisplacementMarker(() -> slide.setSlide(60))
-                .splineToLinearHeading(new Pose2d(50, -10, Math.toRadians(0)), Math.toRadians(0))
-                .build();
 
 
-        TrajectorySequence trajRepeat = drive.trajectorySequenceBuilder(new Pose2d())
-                .addDisplacementMarker(() -> slide.setSlide(480))
-                .splineToLinearHeading(new Pose2d(0, 40, Math.toRadians(90)), Math.toRadians(0))
-                .addDisplacementMarker(slide::grab)
-                .splineToLinearHeading(new Pose2d(0, 0, Math.toRadians(0)), Math.toRadians(0))
-                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .forward(5)
-                .waitSeconds(1.5)
-                .addDisplacementMarker(slide::ungrab)
-                .waitSeconds(1.5)
-                .back(5)
-                .build();
-
         TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(-31.67, -64.77, Math.toRadians(90.00)))
                 .addDisplacementMarker(slide::grab)
                 .splineToConstantHeading(new Vector2d(-12.08, -55.64), Math.toRadians(90.00))
                 .splineToConstantHeading(new Vector2d(-12.27, -12.46), Math.toRadians(90.00))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .splineToConstantHeading(new Vector2d(-29.01, -9), Math.toRadians(180.00))
+                .splineToConstantHeading(new Vector2d(-29.01, -8), Math.toRadians(180.00))
                 .waitSeconds(1)
                 .build();
         drive.setPoseEstimate(trajA.start());
@@ -85,12 +55,12 @@ public void runOpMode() throws InterruptedException {
 
                 .addDisplacementMarker(slide::ungrab)
 
-                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+                .addDisplacementMarker(() -> slide.setSlide(500))
 
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
-                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
-                .addDisplacementMarker(() -> slide.setSlide(500))
-                .lineToSplineHeading(new Pose2d(-65.5, -12, Math.toRadians(180.00)))
+//                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+//                .addDisplacementMarker(() -> slide.setSlide(500))
+                .lineToSplineHeading(new Pose2d(-69, -12, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajC = drive.trajectorySequenceBuilder(trajB.end())
@@ -98,62 +68,114 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(-64.5, -12), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-66.5, -12), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
-                    slide.setSlide(800);
+                    slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
                     sleep(1000);
-
                 })
-//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
-                .lineToSplineHeading(new Pose2d(-27, -9, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
+                .lineToSplineHeading(new Pose2d(-27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
-                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+//                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+//                    sleep(1500);
                     slide.setLinearActuator(ACTUATOR_MAX);
-                    sleep(2000);
+                    sleep(750);
+                    slide.setSlide(LinearSlide.MEDIUM_HEIGHT);
+                    slide.ungrab();
+                    sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(-27, -7), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+//                .lineToConstantHeading(new Vector2d(-26, -5), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+////                .forward(1.5)
+//                .addDisplacementMarker(() -> {
+//
+//                })
+//                .back(2)
+                .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .waitSeconds(0.5)
+                .build();
+        TrajectorySequence trajB2 = drive.trajectorySequenceBuilder(trajC.end())
+
+                .addDisplacementMarker(slide::ungrab)
+
+                .addDisplacementMarker(() -> slide.setSlide(420))
+
+//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
+//                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+//                .addDisplacementMarker(() -> slide.setSlide(500))
+                .lineToSplineHeading(new Pose2d(-68, -11, Math.toRadians(180.00)))
+                .build();
+//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
+        TrajectorySequence trajC2 = drive.trajectorySequenceBuilder(trajB.end())
                 .addDisplacementMarker(() -> {
+                    slide.grab();
+                    sleep(500);
+                })
+                .lineToConstantHeading(new Vector2d(-65.5, -11), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(1000);
+                    slide.setLinearActuator(ACTUATOR_MIN);
+                    sleep(1000);
+                })
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
+                .lineToSplineHeading(new Pose2d(-27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+//                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+//                    sleep(1500);
+                    slide.setLinearActuator(ACTUATOR_MAX);
+                    sleep(750);
+                    slide.setSlide(LinearSlide.MEDIUM_HEIGHT);
                     slide.ungrab();
                     sleep(500);
                 })
+//                .lineToConstantHeading(new Vector2d(-26, -5), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+////                .forward(1.5)
+//                .addDisplacementMarker(() -> {
+//
+//                })
+//                .back(2)
+                .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .waitSeconds(0.5)
                 .build();
         TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
-                .addDisplacementMarker(slide::ungrab)
+//                .addDisplacementMarker(slide::ungrab)
 
-                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+//                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
 
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
                 //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
                 .addDisplacementMarker(() -> slide.setSlide(420))
-                .lineToSplineHeading(new Pose2d(-66.5, -11, Math.toRadians(180.00)))
+                .lineToSplineHeading(new Pose2d(-68, -11, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajD.end())
                 .addDisplacementMarker(() -> {
                     slide.grab();
-                    sleep(500);
+                    sleep(600);
                 })
                 .lineToConstantHeading(new Vector2d(-65.5, -11), SampleMecanumDrive.getVelocityConstraint(5, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
-                    slide.setSlide(500);
+                    slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
-
-                    sleep(1500);
+                    sleep(1000);
                 })
-//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
-                .lineToSplineHeading(new Pose2d(-27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
+                .lineToSplineHeading(new Pose2d(-26, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
-                    slide.setSlide(LinearSlide.MAX_HEIGHT);
                     slide.setLinearActuator(ACTUATOR_MAX);
-                    sleep(2000);
-                })
-                .lineToConstantHeading(new Vector2d(-27, -6), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-                .addDisplacementMarker(() -> {
+                    sleep(750);
+                    slide.setSlide(LinearSlide.MEDIUM_HEIGHT);
                     slide.ungrab();
                     sleep(500);
                 })
+//                .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+//                .addDisplacementMarker(() -> {
+//                    slide.ungrab();
+//                    sleep(500);
+//                })
                 .build();
+
+
         TrajectorySequence trajF = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(slide::ungrab)
 
@@ -177,13 +199,13 @@ public void runOpMode() throws InterruptedException {
                     sleep(1000);
                 })
 //                .addDisplacementMarker(() -> slide.grabAndRaise(800))
-                .lineToSplineHeading(new Pose2d(-27, -6, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToSplineHeading(new Pose2d(-26, -6, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(LinearSlide.MAX_HEIGHT);
                     slide.setLinearActuator(ACTUATOR_MAX);
                     sleep(2000);
                 })
-                .lineToConstantHeading(new Vector2d(-27, -5), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-26, -3), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.ungrab();
                     sleep(500);
@@ -192,13 +214,17 @@ public void runOpMode() throws InterruptedException {
 
         TrajectorySequence trajCenter = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
                     slide.setSlide(MIN_HEIGHT);
                     sleep(1000);
                 })
-                .lineToConstantHeading(new Vector2d(-45, -10))
+                .lineToConstantHeading(new Vector2d(-50, -10))
                 .build();
         TrajectorySequence trajRight = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
                     slide.setSlide(MIN_HEIGHT);
                     sleep(1000);
                 })
@@ -206,10 +232,12 @@ public void runOpMode() throws InterruptedException {
                 .build();
         TrajectorySequence trajLeft = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
                     slide.setSlide(MIN_HEIGHT);
                     sleep(1000);
                 })
-                .lineToConstantHeading(new Vector2d(-70, -10))
+                .lineToConstantHeading(new Vector2d(-74, -10))
                 .build();
         //Cone 1: 480
         //Cone 2: 360
@@ -229,8 +257,10 @@ public void runOpMode() throws InterruptedException {
         drive.followTrajectorySequence(trajB);
         drive.followTrajectorySequence(trajC);
 
-        drive.followTrajectorySequence(trajD);
-        drive.followTrajectorySequence(trajE);
+        drive.followTrajectorySequence(trajB2);
+        drive.followTrajectorySequence(trajC2);
+//        drive.followTrajectorySequence(trajD);
+//        drive.followTrajectorySequence(trajE);
 
         switch (position) {
             case LEFT:

From fb1438975b3ebf728500f78456e227096daa93a2 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 9 Mar 2023 15:46:25 -0500
Subject: [PATCH 39/43] Working 3 Cone Auto

---
 .../ftc/teamcode/auto/AutoSkeleton.java       | 49 ++-----------------
 1 file changed, 5 insertions(+), 44 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
index bf0ef12..fc99dcc 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
@@ -94,7 +94,7 @@ public void runOpMode() throws InterruptedException {
                 .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .waitSeconds(0.5)
                 .build();
-        TrajectorySequence trajB2 = drive.trajectorySequenceBuilder(trajC.end())
+        TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
 
                 .addDisplacementMarker(slide::ungrab)
 
@@ -106,7 +106,7 @@ public void runOpMode() throws InterruptedException {
                 .lineToSplineHeading(new Pose2d(-68, -11, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
-        TrajectorySequence trajC2 = drive.trajectorySequenceBuilder(trajB.end())
+        TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajB.end())
                 .addDisplacementMarker(() -> {
                     slide.grab();
                     sleep(500);
@@ -137,43 +137,6 @@ public void runOpMode() throws InterruptedException {
                 .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .waitSeconds(0.5)
                 .build();
-        TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
-//                .addDisplacementMarker(slide::ungrab)
-
-//                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
-
-//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
-                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
-                .addDisplacementMarker(() -> slide.setSlide(420))
-                .lineToSplineHeading(new Pose2d(-68, -11, Math.toRadians(180.00)))
-                .build();
-//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
-        TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajD.end())
-                .addDisplacementMarker(() -> {
-                    slide.grab();
-                    sleep(600);
-                })
-                .lineToConstantHeading(new Vector2d(-65.5, -11), SampleMecanumDrive.getVelocityConstraint(5, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-                .addDisplacementMarker(() -> {
-                    slide.setSlide(1000);
-                    slide.setLinearActuator(ACTUATOR_MIN);
-                    sleep(1000);
-                })
-                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
-                .lineToSplineHeading(new Pose2d(-26, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-                .addDisplacementMarker(() -> {
-                    slide.setLinearActuator(ACTUATOR_MAX);
-                    sleep(750);
-                    slide.setSlide(LinearSlide.MEDIUM_HEIGHT);
-                    slide.ungrab();
-                    sleep(500);
-                })
-//                .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-//                .addDisplacementMarker(() -> {
-//                    slide.ungrab();
-//                    sleep(500);
-//                })
-                .build();
 
 
         TrajectorySequence trajF = drive.trajectorySequenceBuilder(trajE.end())
@@ -228,7 +191,7 @@ public void runOpMode() throws InterruptedException {
                     slide.setSlide(MIN_HEIGHT);
                     sleep(1000);
                 })
-                .lineToConstantHeading(new Vector2d(-12, -10))
+                .lineToConstantHeading(new Vector2d(-4, -10))
                 .build();
         TrajectorySequence trajLeft = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(() -> {
@@ -257,10 +220,8 @@ public void runOpMode() throws InterruptedException {
         drive.followTrajectorySequence(trajB);
         drive.followTrajectorySequence(trajC);
 
-        drive.followTrajectorySequence(trajB2);
-        drive.followTrajectorySequence(trajC2);
-//        drive.followTrajectorySequence(trajD);
-//        drive.followTrajectorySequence(trajE);
+        drive.followTrajectorySequence(trajD);
+        drive.followTrajectorySequence(trajE);
 
         switch (position) {
             case LEFT:

From b681eb2fbba85779663cebe6fb5dbe94459844a4 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Thu, 9 Mar 2023 16:03:33 -0500
Subject: [PATCH 40/43] Working 3 Cone Auto (Both Sides) (Parking adjusted)

---
 .../auto/{AutoSkeleton.java => AutoLeft.java} |   2 +-
 .../ftc/teamcode/auto/AutoRight.java          | 238 ++++++++++++++++++
 2 files changed, 239 insertions(+), 1 deletion(-)
 rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/{AutoSkeleton.java => AutoLeft.java} (99%)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
similarity index 99%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
index fc99dcc..110d64d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoSkeleton.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
@@ -23,7 +23,7 @@
 
 @Config
 @Autonomous
-public class AutoSkeleton extends LinearOpMode {
+public class AutoLeft extends LinearOpMode {
     public static double x = -27;
     public static double y = -21;
     public static double f = 2;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
new file mode 100644
index 0000000..ceb2bdc
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
@@ -0,0 +1,238 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MAX;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.ACTUATOR_MIN;
+import static org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide.MIN_HEIGHT;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.old.pipeline.SleeveDetectionNewOld;
+import org.firstinspires.ftc.teamcode.subsystem.colorsensor.ColorSensor;
+import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystem.slide.LinearSlide;
+import org.firstinspires.ftc.teamcode.subsystem.webcam.Webcam;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+@Config
+@Autonomous
+public class AutoRight extends LinearOpMode {
+    public static double x = -27;
+    public static double y = -21;
+    public static double f = 2;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        LinearSlide slide = new LinearSlide(hardwareMap);
+        SleeveDetectionNewOld sleeveDetection = new SleeveDetectionNewOld();
+        Webcam cam = new Webcam(hardwareMap, sleeveDetection);
+        ColorSensor colorSensor = new ColorSensor(hardwareMap);
+
+        SleeveDetectionNewOld.ParkingPosition position;
+
+
+        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(31.67, -64.77, Math.toRadians(90.00)))
+                .addDisplacementMarker(slide::grab)
+                .splineToConstantHeading(new Vector2d(12.08, -55.64), Math.toRadians(90.00))
+                .splineToConstantHeading(new Vector2d(12.27, -12.46), Math.toRadians(90.00))
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
+                .splineToConstantHeading(new Vector2d(29.01, -8), Math.toRadians(0))
+                .waitSeconds(1)
+                .build();
+        drive.setPoseEstimate(trajA.start());
+//new Pose2d(-24.38, -7, Math.toRadians(90.00))
+        TrajectorySequence trajB = drive.trajectorySequenceBuilder(trajA.end())
+
+                .addDisplacementMarker(slide::ungrab)
+
+                .addDisplacementMarker(() -> slide.setSlide(500))
+
+//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
+//                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+//                .addDisplacementMarker(() -> slide.setSlide(500))
+                .lineToSplineHeading(new Pose2d(69, -12, Math.toRadians(0)))
+                .build();
+//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
+        TrajectorySequence trajC = drive.trajectorySequenceBuilder(trajB.end())
+                .addDisplacementMarker(() -> {
+                    slide.grab();
+                    sleep(500);
+                })
+                .lineToConstantHeading(new Vector2d(66.5, -12), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(1000);
+                    slide.setLinearActuator(ACTUATOR_MIN);
+                    sleep(1000);
+                })
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
+                .lineToSplineHeading(new Pose2d(27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+//                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+//                    sleep(1500);
+                    slide.setLinearActuator(ACTUATOR_MAX);
+                    sleep(750);
+                    slide.setSlide(LinearSlide.MEDIUM_HEIGHT);
+                    slide.ungrab();
+                    sleep(500);
+                })
+//                .lineToConstantHeading(new Vector2d(-26, -5), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+////                .forward(1.5)
+//                .addDisplacementMarker(() -> {
+//
+//                })
+//                .back(2)
+                .lineToConstantHeading(new Vector2d(26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .waitSeconds(0.5)
+                .build();
+        TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
+
+                .addDisplacementMarker(slide::ungrab)
+
+                .addDisplacementMarker(() -> slide.setSlide(420))
+
+//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
+//                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+//                .addDisplacementMarker(() -> slide.setSlide(500))
+                .lineToSplineHeading(new Pose2d(68, -11, Math.toRadians(0)))
+                .build();
+//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
+        TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajB.end())
+                .addDisplacementMarker(() -> {
+                    slide.grab();
+                    sleep(500);
+                })
+                .lineToConstantHeading(new Vector2d(65.5, -11), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(1000);
+                    slide.setLinearActuator(ACTUATOR_MIN);
+                    sleep(1000);
+                })
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
+                .lineToSplineHeading(new Pose2d(27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+//                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+//                    sleep(1500);
+                    slide.setLinearActuator(ACTUATOR_MAX);
+                    sleep(750);
+                    slide.setSlide(LinearSlide.MEDIUM_HEIGHT);
+                    slide.ungrab();
+                    sleep(500);
+                })
+//                .lineToConstantHeading(new Vector2d(-26, -5), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+////                .forward(1.5)
+//                .addDisplacementMarker(() -> {
+//
+//                })
+//                .back(2)
+                .lineToConstantHeading(new Vector2d(26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .waitSeconds(0.5)
+                .build();
+
+
+        TrajectorySequence trajF = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(slide::ungrab)
+
+                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
+
+//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
+                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
+                .addDisplacementMarker(() -> slide.setSlide(250))
+                .lineToSplineHeading(new Pose2d(66.5, -10, Math.toRadians(0)))
+                .build();
+//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
+        TrajectorySequence trajG = drive.trajectorySequenceBuilder(trajF.end())
+                .addDisplacementMarker(() -> {
+                    slide.grab();
+                    sleep(500);
+                })
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(400);
+                    slide.setLinearActuator(ACTUATOR_MIN);
+
+                    sleep(1000);
+                })
+//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
+                .lineToSplineHeading(new Pose2d(26, -6, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+                    slide.setSlide(LinearSlide.MAX_HEIGHT);
+                    slide.setLinearActuator(ACTUATOR_MAX);
+                    sleep(2000);
+                })
+                .lineToConstantHeading(new Vector2d(26, -3), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
+                })
+                .build();
+
+        TrajectorySequence trajCenter = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
+                    slide.setSlide(MIN_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(50, -10))
+                .build();
+        TrajectorySequence trajRight = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
+                    slide.setSlide(MIN_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(4, -10))
+                .build();
+        TrajectorySequence trajLeft = drive.trajectorySequenceBuilder(trajE.end())
+                .addDisplacementMarker(() -> {
+                    slide.ungrab();
+                    sleep(500);
+                    slide.setSlide(MIN_HEIGHT);
+                    sleep(1000);
+                })
+                .lineToConstantHeading(new Vector2d(74, -10))
+                .build();
+        //Cone 1: 480
+        //Cone 2: 360
+        //Cone 3: 180
+        //Cone 4: 0
+        //Cone 5: 0
+        slide.ungrab();
+        drive.imu.resetYaw();
+        slide.setLinearActuator(ACTUATOR_MAX);
+        waitForStart();
+        position = sleeveDetection.getPosition();
+        if (isStopRequested()) return;
+
+
+        drive.followTrajectorySequence(trajA);
+
+        drive.followTrajectorySequence(trajB);
+        drive.followTrajectorySequence(trajC);
+
+        drive.followTrajectorySequence(trajD);
+        drive.followTrajectorySequence(trajE);
+
+        switch (position) {
+            case LEFT:
+                drive.followTrajectorySequence(trajLeft);
+                break;
+            case CENTER:
+                drive.followTrajectorySequence(trajCenter);
+                break;
+            case RIGHT:
+                drive.followTrajectorySequence(trajRight);
+        }
+
+    }
+}

From 13ebcd3179ffdb5aec53c7b7a80fb504aa02d637 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Fri, 10 Mar 2023 19:06:14 -0500
Subject: [PATCH 41/43] Modified 3 Cone Auto for ground junction (Both Sides)
 (Parking adjusted)

---
 .../ftc/teamcode/auto/AutoLeft.java           | 47 +++----------------
 .../ftc/teamcode/auto/AutoRight.java          | 46 +++---------------
 2 files changed, 12 insertions(+), 81 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
index 110d64d..a50a50c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
@@ -60,6 +60,7 @@ public void runOpMode() throws InterruptedException {
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
 //                .addDisplacementMarker(() -> slide.setSlide(500))
+                .back(5)
                 .lineToSplineHeading(new Pose2d(-69, -12, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
@@ -68,7 +69,7 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(-66.5, -12), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-66.5, -11), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
@@ -103,6 +104,7 @@ public void runOpMode() throws InterruptedException {
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
 //                .addDisplacementMarker(() -> slide.setSlide(500))
+                .back(5)
                 .lineToSplineHeading(new Pose2d(-68, -11, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
@@ -111,14 +113,14 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(-65.5, -11), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-65.5, -9), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
                     sleep(1000);
                 })
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
-                .lineToSplineHeading(new Pose2d(-27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToSplineHeading(new Pose2d(-26, -4, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
 //                    slide.setSlide(LinearSlide.MAX_HEIGHT);
 //                    sleep(1500);
@@ -134,47 +136,10 @@ public void runOpMode() throws InterruptedException {
 //
 //                })
 //                .back(2)
-                .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-25, -1), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .waitSeconds(0.5)
                 .build();
 
-
-        TrajectorySequence trajF = drive.trajectorySequenceBuilder(trajE.end())
-                .addDisplacementMarker(slide::ungrab)
-
-                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
-
-//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
-                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
-                .addDisplacementMarker(() -> slide.setSlide(250))
-                .lineToSplineHeading(new Pose2d(-66.5, -10, Math.toRadians(180.00)))
-                .build();
-//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
-        TrajectorySequence trajG = drive.trajectorySequenceBuilder(trajF.end())
-                .addDisplacementMarker(() -> {
-                    slide.grab();
-                    sleep(500);
-                })
-                .addDisplacementMarker(() -> {
-                    slide.setSlide(400);
-                    slide.setLinearActuator(ACTUATOR_MIN);
-
-                    sleep(1000);
-                })
-//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
-                .lineToSplineHeading(new Pose2d(-26, -6, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-                .addDisplacementMarker(() -> {
-                    slide.setSlide(LinearSlide.MAX_HEIGHT);
-                    slide.setLinearActuator(ACTUATOR_MAX);
-                    sleep(2000);
-                })
-                .lineToConstantHeading(new Vector2d(-26, -3), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-                .addDisplacementMarker(() -> {
-                    slide.ungrab();
-                    sleep(500);
-                })
-                .build();
-
         TrajectorySequence trajCenter = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(() -> {
                     slide.ungrab();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
index ceb2bdc..b079596 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
@@ -56,6 +56,7 @@ public void runOpMode() throws InterruptedException {
                 .addDisplacementMarker(slide::ungrab)
 
                 .addDisplacementMarker(() -> slide.setSlide(500))
+                .back(5)
 
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
@@ -103,6 +104,7 @@ public void runOpMode() throws InterruptedException {
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
 //                .addDisplacementMarker(() -> slide.setSlide(500))
+                .back(5)
                 .lineToSplineHeading(new Pose2d(68, -11, Math.toRadians(0)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
@@ -111,14 +113,14 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(65.5, -11), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(65.5, -9), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
                     sleep(1000);
                 })
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
-                .lineToSplineHeading(new Pose2d(27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToSplineHeading(new Pose2d(26, -4, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
 //                    slide.setSlide(LinearSlide.MAX_HEIGHT);
 //                    sleep(1500);
@@ -134,47 +136,11 @@ public void runOpMode() throws InterruptedException {
 //
 //                })
 //                .back(2)
-                .lineToConstantHeading(new Vector2d(26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(25, -1), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .waitSeconds(0.5)
                 .build();
 
-
-        TrajectorySequence trajF = drive.trajectorySequenceBuilder(trajE.end())
-                .addDisplacementMarker(slide::ungrab)
-
-                .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MIN_HEIGHT))
-
-//                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
-                //.splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
-                .addDisplacementMarker(() -> slide.setSlide(250))
-                .lineToSplineHeading(new Pose2d(66.5, -10, Math.toRadians(0)))
-                .build();
-//new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
-        TrajectorySequence trajG = drive.trajectorySequenceBuilder(trajF.end())
-                .addDisplacementMarker(() -> {
-                    slide.grab();
-                    sleep(500);
-                })
-                .addDisplacementMarker(() -> {
-                    slide.setSlide(400);
-                    slide.setLinearActuator(ACTUATOR_MIN);
-
-                    sleep(1000);
-                })
-//                .addDisplacementMarker(() -> slide.grabAndRaise(800))
-                .lineToSplineHeading(new Pose2d(26, -6, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-                .addDisplacementMarker(() -> {
-                    slide.setSlide(LinearSlide.MAX_HEIGHT);
-                    slide.setLinearActuator(ACTUATOR_MAX);
-                    sleep(2000);
-                })
-                .lineToConstantHeading(new Vector2d(26, -3), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-                .addDisplacementMarker(() -> {
-                    slide.ungrab();
-                    sleep(500);
-                })
-                .build();
-
+        
         TrajectorySequence trajCenter = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(() -> {
                     slide.ungrab();

From 338586066fd1a70a226358298ca3421550399226 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Fri, 10 Mar 2023 21:54:28 -0500
Subject: [PATCH 42/43] Added actuator to old teleop

---
 .../teamcode/old/subsystem/LinearSlide.java    | 18 ++++++++++++++++++
 .../old/teleop/TwoPlayerTeleopOld.java         |  1 +
 2 files changed, 19 insertions(+)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
index 2b68e85..86d957c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
@@ -21,9 +21,13 @@ public class LinearSlide extends Subsystem {
     public static final double GRIP_MIN = 0;
     public static final double GRIP_MAX = 0.6;
 
+    public static double ACTUATOR_MIN = 0.4;
+    public static double ACTUATOR_MAX = 0.67;
+
     // Hardware
     public DcMotorEx slideMotor;
     public Servo leftGripper, rightGripper;
+    public Servo linearActuator;
 
     private boolean gripperClosed = false;
     private boolean dPadPressed = false;
@@ -101,6 +105,14 @@ public void readGamepad() {
             gripperClosed = false;
         }
 
+        if (opMode.gamepad2.right_trigger > 0.5) {
+            setLinearActuator(ACTUATOR_MIN);
+
+        } else if (opMode.gamepad2.left_trigger > 0.5) {
+            setLinearActuator(ACTUATOR_MAX);
+
+        }
+
         if (gripperClosed) {
             grab();
         } else {
@@ -142,4 +154,10 @@ public void setSlideAndWait(int steps) {
         setSlide(steps);
         opMode.blockOn(slideMotor);
     }
+
+    public void setLinearActuator(double steps) {
+        linearActuator.setPosition(steps);
+    }
+
+
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java
index bab2bf7..56b60f5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/teleop/TwoPlayerTeleopOld.java
@@ -14,6 +14,7 @@ public class TwoPlayerTeleopOld extends RobotOpMode {
     @Override
     public void runOpMode() {
         addSubsystems(drive, slide);
+        slide.setLinearActuator(LinearSlide.GRIP_MAX);
         waitForStart();
 
         while (opModeIsActive()) {

From ffbe2c3883f823924c9b86e09a6608c2529b15e0 Mon Sep 17 00:00:00 2001
From: largebread72 <ayushkayal2006@gmail.com>
Date: Tue, 14 Mar 2023 13:51:03 -0400
Subject: [PATCH 43/43] States autonomous adjustments

---
 .../ftc/teamcode/auto/AutoLeft.java           | 29 ++++++++++--------
 .../ftc/teamcode/auto/AutoRight.java          | 30 ++++++++++---------
 .../teamcode/old/subsystem/LinearSlide.java   |  2 ++
 3 files changed, 34 insertions(+), 27 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
index a50a50c..0fd7091 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoLeft.java
@@ -41,12 +41,14 @@ public void runOpMode() throws InterruptedException {
         SleeveDetectionNewOld.ParkingPosition position;
 
 
-        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(-31.67, -64.77, Math.toRadians(90.00)))
+        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(-33.67, -66.77, Math.toRadians(90.00)))
                 .addDisplacementMarker(slide::grab)
-                .splineToConstantHeading(new Vector2d(-12.08, -55.64), Math.toRadians(90.00))
-                .splineToConstantHeading(new Vector2d(-12.27, -12.46), Math.toRadians(90.00))
+//                .splineToConstantHeading(new Vector2d(-12.08, -55.64), Math.toRadians(90.00))
+//                .splineToConstantHeading(new Vector2d(-12.27, -12.46), Math.toRadians(90.00))
+                .splineToConstantHeading(new Vector2d(-13, -64.77), Math.toRadians(90.00))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .splineToConstantHeading(new Vector2d(-29.01, -8), Math.toRadians(180.00))
+                .lineToConstantHeading(new Vector2d(-13, -9.5))
+                .lineToConstantHeading(new Vector2d(-25.51, -9.5))
                 .waitSeconds(1)
                 .build();
         drive.setPoseEstimate(trajA.start());
@@ -61,7 +63,7 @@ public void runOpMode() throws InterruptedException {
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
 //                .addDisplacementMarker(() -> slide.setSlide(500))
                 .back(5)
-                .lineToSplineHeading(new Pose2d(-69, -12, Math.toRadians(180.00)))
+                .lineToSplineHeading(new Pose2d(-69, -10.5, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajC = drive.trajectorySequenceBuilder(trajB.end())
@@ -69,14 +71,14 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(-66.5, -11), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-66.5, -9.5), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
                     sleep(1000);
                 })
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
-                .lineToSplineHeading(new Pose2d(-27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToSplineHeading(new Pose2d(-25, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
 //                    slide.setSlide(LinearSlide.MAX_HEIGHT);
 //                    sleep(1500);
@@ -92,7 +94,7 @@ public void runOpMode() throws InterruptedException {
 //
 //                })
 //                .back(2)
-                .lineToConstantHeading(new Vector2d(-26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-24, -4), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .waitSeconds(0.5)
                 .build();
         TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
@@ -105,7 +107,7 @@ public void runOpMode() throws InterruptedException {
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
 //                .addDisplacementMarker(() -> slide.setSlide(500))
                 .back(5)
-                .lineToSplineHeading(new Pose2d(-68, -11, Math.toRadians(180.00)))
+                .lineToSplineHeading(new Pose2d(-68, -10, Math.toRadians(180.00)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajB.end())
@@ -113,14 +115,14 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(-65.5, -9), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-65.5, -9), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
                     sleep(1000);
                 })
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
-                .lineToSplineHeading(new Pose2d(-26, -4, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToSplineHeading(new Pose2d(-24, -5, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
 //                    slide.setSlide(LinearSlide.MAX_HEIGHT);
 //                    sleep(1500);
@@ -136,7 +138,8 @@ public void runOpMode() throws InterruptedException {
 //
 //                })
 //                .back(2)
-                .lineToConstantHeading(new Vector2d(-25, -1), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(-23, -3), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .addDisplacementMarker(slide::grab)
                 .waitSeconds(0.5)
                 .build();
 
@@ -165,7 +168,7 @@ public void runOpMode() throws InterruptedException {
                     slide.setSlide(MIN_HEIGHT);
                     sleep(1000);
                 })
-                .lineToConstantHeading(new Vector2d(-74, -10))
+                .lineToConstantHeading(new Vector2d(-72, -10))
                 .build();
         //Cone 1: 480
         //Cone 2: 360
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
index b079596..081cd79 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutoRight.java
@@ -41,12 +41,14 @@ public void runOpMode() throws InterruptedException {
         SleeveDetectionNewOld.ParkingPosition position;
 
 
-        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(31.67, -64.77, Math.toRadians(90.00)))
+        TrajectorySequence trajA = drive.trajectorySequenceBuilder(new Pose2d(33.67, -66.77, Math.toRadians(90.00)))
                 .addDisplacementMarker(slide::grab)
-                .splineToConstantHeading(new Vector2d(12.08, -55.64), Math.toRadians(90.00))
-                .splineToConstantHeading(new Vector2d(12.27, -12.46), Math.toRadians(90.00))
+//                .splineToConstantHeading(new Vector2d(12.08, -55.64), Math.toRadians(90.00))
+//                .splineToConstantHeading(new Vector2d(12.27, -12.46), Math.toRadians(90.00))
+                .splineToConstantHeading(new Vector2d(13, -64.77), Math.toRadians(90.00))
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT))
-                .splineToConstantHeading(new Vector2d(29.01, -8), Math.toRadians(0))
+                .lineToConstantHeading(new Vector2d(13, -9.5))
+                .splineToConstantHeading(new Vector2d(25.51, -9.5), Math.toRadians(90.00))
                 .waitSeconds(1)
                 .build();
         drive.setPoseEstimate(trajA.start());
@@ -61,7 +63,7 @@ public void runOpMode() throws InterruptedException {
 //                .lineToConstantHeading(new Vector2d(-24.38, -10.00))
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
 //                .addDisplacementMarker(() -> slide.setSlide(500))
-                .lineToSplineHeading(new Pose2d(69, -12, Math.toRadians(0)))
+                .lineToSplineHeading(new Pose2d(69, -10.5, Math.toRadians(0)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajC = drive.trajectorySequenceBuilder(trajB.end())
@@ -69,14 +71,14 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(66.5, -12), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(66.5, -9), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
                     sleep(1000);
                 })
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
-                .lineToSplineHeading(new Pose2d(27, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToSplineHeading(new Pose2d(25, -7, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
 //                    slide.setSlide(LinearSlide.MAX_HEIGHT);
 //                    sleep(1500);
@@ -92,7 +94,7 @@ public void runOpMode() throws InterruptedException {
 //
 //                })
 //                .back(2)
-                .lineToConstantHeading(new Vector2d(26, -4), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(24, -4), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .waitSeconds(0.5)
                 .build();
         TrajectorySequence trajD = drive.trajectorySequenceBuilder(trajC.end())
@@ -105,7 +107,7 @@ public void runOpMode() throws InterruptedException {
 //                .splineToConstantHeading(new Vector2d(-45.61, -10.72), Math.toRadians(180.00))
 //                .addDisplacementMarker(() -> slide.setSlide(500))
                 .back(5)
-                .lineToSplineHeading(new Pose2d(68, -11, Math.toRadians(0)))
+                .lineToSplineHeading(new Pose2d(68, -10, Math.toRadians(0)))
                 .build();
 //new Pose2d(-67.00, -7.78, Math.toRadians(180.00))
         TrajectorySequence trajE = drive.trajectorySequenceBuilder(trajB.end())
@@ -113,14 +115,14 @@ public void runOpMode() throws InterruptedException {
                     slide.grab();
                     sleep(500);
                 })
-                .lineToConstantHeading(new Vector2d(65.5, -9), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(65.5, -9), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
                     slide.setSlide(1000);
                     slide.setLinearActuator(ACTUATOR_MIN);
                     sleep(1000);
                 })
                 .addDisplacementMarker(() -> slide.setSlide(LinearSlide.MAX_HEIGHT - 300))
-                .lineToSplineHeading(new Pose2d(26, -4, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToSplineHeading(new Pose2d(24, -5, Math.toRadians(90.00)), SampleMecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .addDisplacementMarker(() -> {
 //                    slide.setSlide(LinearSlide.MAX_HEIGHT);
 //                    sleep(1500);
@@ -136,11 +138,11 @@ public void runOpMode() throws InterruptedException {
 //
 //                })
 //                .back(2)
-                .lineToConstantHeading(new Vector2d(25, -1), SampleMecanumDrive.getVelocityConstraint(10, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .lineToConstantHeading(new Vector2d(23, -3), SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                 .waitSeconds(0.5)
                 .build();
 
-        
+
         TrajectorySequence trajCenter = drive.trajectorySequenceBuilder(trajE.end())
                 .addDisplacementMarker(() -> {
                     slide.ungrab();
@@ -166,7 +168,7 @@ public void runOpMode() throws InterruptedException {
                     slide.setSlide(MIN_HEIGHT);
                     sleep(1000);
                 })
-                .lineToConstantHeading(new Vector2d(74, -10))
+                .lineToConstantHeading(new Vector2d(72, -10))
                 .build();
         //Cone 1: 480
         //Cone 2: 360
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
index 86d957c..f1b8859 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/old/subsystem/LinearSlide.java
@@ -51,6 +51,8 @@ public void setup() {
         // Initialize gripper servos
         leftGripper = opMode.hardwareMap.get(Servo.class, "grip1");
         rightGripper = opMode.hardwareMap.get(Servo.class, "grip2");
+
+        linearActuator = opMode.hardwareMap.get(Servo.class, "linear_actuator");
     }
 
     public void updateTelemetry() {