diff --git a/.run/AutoPush.run.xml b/.run/AutoPush.run.xml
index 8730f5c..01295e5 100644
--- a/.run/AutoPush.run.xml
+++ b/.run/AutoPush.run.xml
@@ -1,6 +1,6 @@
 <component name="ProjectRunConfigurationManager">
   <configuration default="false" name="AutoPush" type="ShConfigurationType">
-    <option name="SCRIPT_TEXT" value="adb push PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json sdcard/FIRST/positions/blue/close-basket.json; adb push PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json sdcard/FIRST/positions/blue/far-basket.json; adb push PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json sdcard/FIRST/positions/red/close-basket.json; adb push PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json sdcard/FIRST/positions/red/far-basket.json; " />
+    <option name="SCRIPT_TEXT" value="adb push TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec4.pp sdcard/FIRST/positions/spec4.pp; adb push TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5.pp sdcard/FIRST/positions/spec5.pp; adb push TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5sweep.pp sdcard/FIRST/positions/spec5sweep.pp;" />
     <option name="INDEPENDENT_SCRIPT_PATH" value="true" />
     <option name="SCRIPT_PATH" value="" />
     <option name="SCRIPT_OPTIONS" value="" />
diff --git a/.run/PathVisualizer.run.xml b/.run/PathVisualizer.run.xml
deleted file mode 100644
index e5ce65d..0000000
--- a/.run/PathVisualizer.run.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<component name="ProjectRunConfigurationManager">
-  <configuration default="false" name="PathVisualizer" type="Application" factoryName="Application">
-    <option name="MAIN_CLASS_NAME" value="com.example.meepmeep.PathVisualizer" />
-    <module name="deep.PathVisualizer.main" />
-    <method v="2">
-      <option name="Make" enabled="true" />
-    </method>
-  </configuration>
-</component>
\ No newline at end of file
diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml
index 1ce6a3e..43b5e8c 100644
--- a/FtcRobotController/src/main/AndroidManifest.xml
+++ b/FtcRobotController/src/main/AndroidManifest.xml
@@ -1,79 +1,80 @@
 <?xml version="1.0" encoding="utf-8"?>
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
-          xmlns:tools="http://schemas.android.com/tools"
-    android:versionCode="57"
-          android:versionName="10.1.1">
-
-  <uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />
-
-  <application
-    android:allowBackup="true"
-    android:largeHeap="true"
-    android:icon="@drawable/ic_launcher"
-    android:label="@string/app_name"
-    android:theme="@style/AppThemeRedRC"
-    android:usesCleartextTraffic="true">
-
-    <!-- Indicates to the ControlHubUpdater what the latest version of the Control Hub is that this app supports -->
-    <meta-data
-        android:name="org.firstinspires.latestSupportedControlHubVersion"
-        android:value="1" />
-
-    <!-- The main robot controller activity -->
-    <activity android:name="org.firstinspires.ftc.robotcontroller.internal.PermissionValidatorWrapper"
-              android:screenOrientation="fullUser"
-              android:configChanges="orientation|screenSize"
-              android:label="@string/app_name"
-              android:launchMode="singleTask" >
-
-      <intent-filter>
-        <category android:name="android.intent.category.LAUNCHER" />
-        <action android:name="android.intent.action.MAIN" />
-      </intent-filter>
-
-    </activity>
-
-    <activity
-      android:name="org.firstinspires.ftc.robotcontroller.internal.FtcRobotControllerActivity"
-      android:screenOrientation="fullUser"
-      android:configChanges="orientation|screenSize"
-      android:label="@string/app_name"
-      android:launchMode="singleTask" >
-
-      <intent-filter>
-        <action android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" />
-      </intent-filter>
-
-      <meta-data
-        android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED"
-        android:resource="@xml/device_filter" />
-
-      <!--org.firstinspires.main.entry indicates that this app is compatible with the Dragonboard Control Hub-->
-      <meta-data
-          android:name="org.firstinspires.main.entry"
-          android:value="true" />
-    </activity>
-
-    <!-- The robot controller service in which most of the robot functionality is managed -->
-    <service
-      android:name="com.qualcomm.ftccommon.FtcRobotControllerService"
-      android:enabled="true" />
-
-
-    <!-- Assistant that autostarts the robot controller on android boot (if it's supposed to) -->
-    <receiver
-      android:enabled="true"
-      android:exported="true"
-      android:name="org.firstinspires.ftc.ftccommon.internal.RunOnBoot"
-      android:permission="android.permission.RECEIVE_BOOT_COMPLETED">
-
-      <intent-filter>
-        <category android:name="android.intent.category.DEFAULT" />
-        <action android:name="android.intent.action.BOOT_COMPLETED" />
-        <action android:name="android.intent.action.QUICKBOOT_POWERON" />
-      </intent-filter>
-    </receiver>
-
-  </application>
-
-</manifest>
+    android:versionCode="58"
+    android:versionName="10.2">
+
+    <uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />
+
+    <application
+        android:allowBackup="true"
+        android:largeHeap="true"
+        android:icon="@drawable/ic_launcher"
+        android:label="@string/app_name"
+        android:theme="@style/AppThemeRedRC"
+        android:usesCleartextTraffic="true">
+
+        <!-- Indicates to the ControlHubUpdater what the latest version of the Control Hub is that this app supports -->
+        <meta-data
+            android:name="org.firstinspires.latestSupportedControlHubVersion"
+            android:value="1" />
+
+        <!-- The main robot controller activity -->
+        <activity
+            android:name="org.firstinspires.ftc.robotcontroller.internal.PermissionValidatorWrapper"
+            android:screenOrientation="fullUser"
+            android:configChanges="orientation|screenSize"
+            android:label="@string/app_name"
+            android:launchMode="singleTask">
+
+            <intent-filter>
+                <category android:name="android.intent.category.LAUNCHER" />
+                <action android:name="android.intent.action.MAIN" />
+            </intent-filter>
+
+        </activity>
+
+        <activity
+            android:name="org.firstinspires.ftc.robotcontroller.internal.FtcRobotControllerActivity"
+            android:screenOrientation="fullUser"
+            android:configChanges="orientation|screenSize"
+            android:label="@string/app_name"
+            android:launchMode="singleTask">
+
+            <intent-filter>
+                <action android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED" />
+            </intent-filter>
+
+            <meta-data
+                android:name="android.hardware.usb.action.USB_DEVICE_ATTACHED"
+                android:resource="@xml/device_filter" />
+
+            <!--org.firstinspires.main.entry indicates that this app is compatible with the Dragonboard Control Hub-->
+            <meta-data
+                android:name="org.firstinspires.main.entry"
+                android:value="true" />
+        </activity>
+
+        <!-- The robot controller service in which most of the robot functionality is managed -->
+        <service
+            android:name="com.qualcomm.ftccommon.FtcRobotControllerService"
+            android:enabled="true" />
+
+
+        <!-- Assistant that autostarts the robot controller on android boot (if it's supposed to) -->
+        <receiver
+            android:enabled="true"
+            android:exported="true"
+            android:name="org.firstinspires.ftc.ftccommon.internal.RunOnBoot"
+            android:permission="android.permission.RECEIVE_BOOT_COMPLETED">
+
+            <intent-filter>
+                <category android:name="android.intent.category.DEFAULT" />
+
+                <action android:name="android.intent.action.BOOT_COMPLETED" />
+                <action android:name="android.intent.action.QUICKBOOT_POWERON" />
+            </intent-filter>
+        </receiver>
+
+    </application>
+
+</manifest>
\ No newline at end of file
diff --git a/PINPOINT_INSTRUCTIONS.md b/PINPOINT_INSTRUCTIONS.md
deleted file mode 100644
index c198235..0000000
--- a/PINPOINT_INSTRUCTIONS.md
+++ /dev/null
@@ -1,9 +0,0 @@
-Tuning steps are mostly the same as https://rr.brott.dev/docs/v1-0/tuning/
-differences: 
-ensure you  configure the Pinpoint in hardwaremap as pinpoint by default or change in PinpointDrive
-Leave inPerTick at 1.0 in MecanumDrive and skip ForwardPushTest; instead configure encoder resolution in PinpointDrive to one of the presets for gobilda odometry
-Also, ensure that you use DeadWheelDirectionDebugger and properly reverse the wheels in PinpointDrive 
-Also change the drive class in TuningOpModes, SplineTest, and your op modes to PinpointDrive
-Then you can move on with tuning in https://rr.brott.dev/docs/v1-0/tuning/ as normal.
-
-TODO: expand these docs
diff --git a/PathVisualizer/.gitignore b/PathVisualizer/.gitignore
deleted file mode 100644
index 42afabf..0000000
--- a/PathVisualizer/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-/build
\ No newline at end of file
diff --git a/PathVisualizer/build.gradle b/PathVisualizer/build.gradle
deleted file mode 100644
index 1e3e68c..0000000
--- a/PathVisualizer/build.gradle
+++ /dev/null
@@ -1,17 +0,0 @@
-plugins {
-    id 'java-library'
-}
-
-java {
-    sourceCompatibility = JavaVersion.VERSION_1_7
-    targetCompatibility = JavaVersion.VERSION_1_7
-}
-
-repositories {
-    maven { url = 'https://maven.brott.dev/' }
-}
-
-dependencies {
-    implementation 'com.acmerobotics.roadrunner:MeepMeep:0.1.6'
-    implementation 'org.json:json:20240303'
-}
\ No newline at end of file
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java b/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java
deleted file mode 100644
index ef211d2..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java
+++ /dev/null
@@ -1,37 +0,0 @@
-package com.example.meepmeep;
-
-import com.example.meepmeep.trajectories.BlueCloseBasket;
-import com.example.meepmeep.trajectories.BlueFarBasket;
-import com.example.meepmeep.trajectories.BlueFarPush;
-import com.example.meepmeep.trajectories.RedCloseBasket;
-import com.example.meepmeep.trajectories.RedFarBasket;
-import com.example.meepmeep.trajectories.RedFarPush;
-import com.noahbres.meepmeep.MeepMeep;
-import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder;
-import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity;
-
-public class PathVisualizer {
-    public static void main(String[] args) {
-        MeepMeep meepMeep = new MeepMeep(700);
-
-        RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
-                .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15)
-                .build();
-
-
-        // BlueCloseBasket trajectories = new BlueCloseBasket(0);
-        // BlueFarBasket trajectories = new BlueFarBasket(0);
-        // BlueFarPush trajectories = new BlueFarPush(0);
-        // RedCloseBasket trajectories = new RedCloseBasket(0);
-        // RedFarBasket trajectories = new RedFarBasket(0);
-         RedFarPush trajectories = new RedFarPush(0);
-
-        myBot.runAction(trajectories.start(myBot.getDrive().actionBuilder(trajectories.getStart())));
-
-        meepMeep.setBackground(MeepMeep.Background.FIELD_INTO_THE_DEEP_JUICE_DARK)
-                .setDarkMode(true)
-                .setBackgroundAlpha(0.95f)
-                .addEntity(myBot)
-                .start();
-    }
-}
\ No newline at end of file
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json b/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json
deleted file mode 100644
index 2ca288d..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json
+++ /dev/null
@@ -1,42 +0,0 @@
-{
-  "START": {
-    "x": 34.5,
-    "y": 64.0,
-    "heading": 270.0
-  },
-  "SCORE": {
-    "x": 55.0,
-    "y": 55.0,
-    "heading": 45.0
-  },
-  "RUNG": {
-    "x": 0.0,
-    "y": 38.0,
-    "heading": 270.0
-  },
-  "LEFT_SPIKEMARK": {
-    "x": 58.0,
-    "y": 49.0,
-    "heading": 63.5
-  },
-  "MID_SPIKEMARK": {
-    "x": 58.0,
-    "y": 49.0,
-    "heading": 90.0
-  },
-  "RIGHT_SPIKEMARK": {
-    "x": 58.0,
-    "y": 49.0,
-    "heading": 116.5
-  },
-  "SUBMERSIBLE": {
-    "x": 34.5,
-    "y": 11.5,
-    "heading": 0.0
-  },
-  "PARK": {
-    "x": -46.0,
-    "y": 58.0,
-    "heading": 270
-  }
-}
\ No newline at end of file
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json b/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json
deleted file mode 100644
index 3d9fba0..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json
+++ /dev/null
@@ -1,72 +0,0 @@
-{
-  "START": {
-    "x": -23.0,
-    "y": 64.0,
-    "heading": 270.0
-  },
-  "RUNG": {
-    "x": 0.0,
-    "y": 38.0,
-    "heading": 270.0
-  },
-  "RUNG1": {
-    "x": -2,
-    "y": 38.0,
-    "heading": 270.0
-  },
-  "RUNG2": {
-    "x": -4,
-    "y": 38.0,
-    "heading": 270.0
-  },
-  "RUNG3": {
-    "x": -6,
-    "y": 38.0,
-    "heading": 270.0
-  },
-  "MID_SPIKEMARK": {
-    "x": -58.0,
-    "y": 49.0,
-    "heading": 90.0
-  },
-  "LEFT_SPIKEMARK": {
-    "x": -58.0,
-    "y": 49.0,
-    "heading": 63.5
-  },
-  "RIGHT_SPIKEMARK": {
-    "x": -58.0,
-    "y": 49.0,
-    "heading": 116.5
-  },
-  "SCORE": {
-    "x": -23.0,
-    "y": 49.0,
-    "heading": 315
-  },
-  "PARK": {
-    "x": -46.0,
-    "y": 58.0,
-    "heading": 270
-  },
-  "PUSH1": {
-    "x": -46,
-    "y": 12,
-    "heading": 90
-  },
-  "PUSH2": {
-    "x": -56,
-    "y": 12,
-    "heading": 90
-  },
-  "PUSH3": {
-    "x": -62,
-    "y": 12,
-    "heading": 90
-  },
-  "SCOREPUSH": {
-    "x": -26.0,
-    "y": 55.0,
-    "heading": 315
-  }
-}
\ No newline at end of file
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/constants.txt b/PathVisualizer/src/main/java/com/example/meepmeep/positions/constants.txt
deleted file mode 100644
index 8e4906f..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/constants.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-For reference to calculate certain positions
-
-ROBOT_WIDTH = 15.3
-ROBOT_LENGTH = 16
-CENTER_TO_CAMERA = 213.052 * 0.0394
-TILE_LENGTH = 23.0 // excluding "tabs"
-FIELD_LENGTH = 72.0
\ No newline at end of file
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json b/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json
deleted file mode 100644
index 979fa26..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json
+++ /dev/null
@@ -1,42 +0,0 @@
-{
-  "START": {
-    "x": -34.5,
-    "y": -64.0,
-    "heading": 90.0
-  },
-  "MID_SPIKEMARK": {
-    "x": -58.0,
-    "y": -49.0,
-    "heading": 270.0
-  },
-  "LEFT_SPIKEMARK": {
-    "x": -58.0,
-    "y": -49.0,
-    "heading": 243.5
-  },
-  "RIGHT_SPIKEMARK": {
-    "x": -58.0,
-    "y": -49.0,
-    "heading": -63.5
-  },
-  "RUNG": {
-    "x": 0.0,
-    "y": -38.0,
-    "heading": 90.0
-  },
-  "SCORE": {
-    "x": -55.0,
-    "y": -55.0,
-    "heading": 225.0
-  },
-  "SUBMERSIBLE": {
-    "x": -34.5,
-    "y": -11.5,
-    "heading": 180.0
-  },
-  "PARK": {
-    "x": 46.0,
-    "y": -58,
-    "heading": 90
-  }
-}
\ No newline at end of file
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json b/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json
deleted file mode 100644
index 211c227..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json
+++ /dev/null
@@ -1,72 +0,0 @@
-{
-  "START": {
-    "x": 23,
-    "y": -64.0,
-    "heading": 90.0
-  },
-  "RUNG": {
-    "x": 0.0,
-    "y": -38.0,
-    "heading": 90.0
-  },
-  "RUNG1": {
-    "x": 3.0,
-    "y": -38.0,
-    "heading": 90.0
-  },
-  "RUNG2": {
-    "x": 6.0,
-    "y": -38.0,
-    "heading": 90.0
-  },
-  "RUNG3": {
-    "x": 9.0,
-    "y": -38.0,
-    "heading": 90.0
-  },
-  "MID_SPIKEMARK": {
-    "x": 58.0,
-    "y": -49.0,
-    "heading": 270.0
-  },
-  "LEFT_SPIKEMARK": {
-    "x": 58.0,
-    "y": -49.0,
-    "heading": 243.5
-  },
-  "RIGHT_SPIKEMARK": {
-    "x": 58.0,
-    "y": -49.0,
-    "heading": -63.5
-  },
-  "SCORE": {
-    "x": 23.0,
-    "y": -49.0,
-    "heading": 135
-  },
-  "PARK": {
-    "x": 46.0,
-    "y": -58,
-    "heading": 90
-  },
-  "PUSH1": {
-    "x": 46,
-    "y": -12,
-    "heading": 270
-  },
-  "PUSH2": {
-    "x": 56,
-    "y": -12,
-    "heading": 270
-  },
-  "PUSH3": {
-    "x": 62,
-    "y": -12,
-    "heading": 270
-  },
-  "SCOREPUSH": {
-    "x": 26.0,
-    "y": -55.0,
-    "heading": 135
-  }
-}
\ No newline at end of file
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java
deleted file mode 100644
index 3cc57be..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java
+++ /dev/null
@@ -1,115 +0,0 @@
-package com.example.meepmeep.trajectories;
-
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-
-import org.json.JSONObject;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.Scanner;
-
-public class BlueCloseBasket {
-    private final Pose2d START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, SUBMERSIBLE, PARK;
-
-    public BlueCloseBasket(int type) {
-        String jsonString = "";
-
-        try {
-            File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/close-basket.json" : "/sdcard/FIRST/positions/blue/close-basket.json"));
-            Scanner reader = new Scanner(file);
-
-            while (reader.hasNextLine()) {
-                jsonString += reader.nextLine();
-            }
-        } catch (
-                FileNotFoundException e) {
-            e.printStackTrace();
-        }
-
-        JSONObject positions = new JSONObject(jsonString);
-
-        this.START = new Pose2d(
-                positions.getJSONObject("START").getDouble("x"),
-                positions.getJSONObject("START").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("START").getDouble("heading"))
-        );
-
-        this.RUNGS = new Pose2d(
-                positions.getJSONObject("RUNG").getDouble("x"),
-                positions.getJSONObject("RUNG").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading"))
-        );
-
-        this.LEFT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.MID_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.RIGHT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.SCORE = new Pose2d(
-                positions.getJSONObject("SCORE").getDouble("x"),
-                positions.getJSONObject("SCORE").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading"))
-        );
-
-        this.SUBMERSIBLE = new Pose2d(
-                positions.getJSONObject("SUBMERSIBLE").getDouble("x"),
-                positions.getJSONObject("SUBMERSIBLE").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SUBMERSIBLE").getDouble("heading"))
-        );
-
-        this.PARK = new Pose2d(
-                positions.getJSONObject("PARK").getDouble("x"),
-                positions.getJSONObject("PARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PARK").getDouble("heading"))
-        );
-
-    }
-
-    public Action start(TrajectoryActionBuilder builder) {
-        return builder.splineToLinearHeading(RUNGS, Math.toRadians(180))
-                .setTangent(Math.toRadians(0))
-                .splineToLinearHeading(MID_SPIKEMARK, Math.toRadians(30))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(SCORE, Math.toRadians(93))
-                .splineToLinearHeading(LEFT_SPIKEMARK, Math.toRadians(0))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(SCORE, Math.toRadians(93))
-                .splineToLinearHeading(RIGHT_SPIKEMARK, Math.toRadians(0))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(SCORE, Math.toRadians(93))
-                .build();
-    }
-
-    public Action cycle(TrajectoryActionBuilder builder){
-        return builder.setTangent(Math.toRadians(180))
-                .splineToLinearHeading(SUBMERSIBLE, Math.toRadians(270))
-                .setTangent(Math.toRadians(90))
-                .splineToLinearHeading(SCORE, Math.toRadians(0))
-                .build();
-    }
-
-    public Action park(TrajectoryActionBuilder builder){
-        return builder.setTangent(Math.toRadians(180))
-                .splineToLinearHeading(PARK, Math.toRadians(180))
-                .build();
-    }
-
-    public Pose2d getStart() {
-        return this.START;
-    }
-}
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java
deleted file mode 100644
index ff49cd8..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java
+++ /dev/null
@@ -1,125 +0,0 @@
-package com.example.meepmeep.trajectories;
-
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-
-import org.json.JSONObject;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.Scanner;
-
-public class BlueFarBasket {
-    private final Pose2d START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK;
-
-    public BlueFarBasket(int type) {
-        String jsonString = "";
-
-        try {
-            File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json" : "/sdcard/FIRST/positions/blue/far-basket.json"));
-            Scanner reader = new Scanner(file);
-
-            while (reader.hasNextLine()) {
-                jsonString += reader.nextLine();
-            }
-        } catch (
-                FileNotFoundException e) {
-            e.printStackTrace();
-        }
-
-        JSONObject positions = new JSONObject(jsonString);
-
-        this.START = new Pose2d(
-                positions.getJSONObject("START").getDouble("x"),
-                positions.getJSONObject("START").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("START").getDouble("heading"))
-        );
-
-        this.RUNGS = new Pose2d(
-                positions.getJSONObject("RUNG").getDouble("x"),
-                positions.getJSONObject("RUNG").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading"))
-        );
-
-        this.RUNGS1 = new Pose2d(
-                positions.getJSONObject("RUNG1").getDouble("x"),
-                positions.getJSONObject("RUNG1").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG1").getDouble("heading"))
-        );
-
-        this.RUNGS2 = new Pose2d(
-                positions.getJSONObject("RUNG2").getDouble("x"),
-                positions.getJSONObject("RUNG2").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG2").getDouble("heading"))
-        );
-
-        this.RUNGS3 = new Pose2d(
-                positions.getJSONObject("RUNG3").getDouble("x"),
-                positions.getJSONObject("RUNG3").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG3").getDouble("heading"))
-        );
-
-        this.LEFT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.MID_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.RIGHT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.SCORE = new Pose2d(
-                positions.getJSONObject("SCORE").getDouble("x"),
-                positions.getJSONObject("SCORE").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading"))
-        );
-
-        this.PARK = new Pose2d(
-                positions.getJSONObject("PARK").getDouble("x"),
-                positions.getJSONObject("PARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PARK").getDouble("heading"))
-        );
-    }
-
-    public Action start(TrajectoryActionBuilder builder) {
-        return builder.splineToLinearHeading(this.RUNGS, Math.toRadians(0))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.MID_SPIKEMARK, Math.toRadians(150))
-                .turnTo(this.LEFT_SPIKEMARK.heading)
-                .turnTo(Math.toRadians(90))
-                .turnTo(this.RIGHT_SPIKEMARK.heading)
-                .turnTo(Math.toRadians(90))
-                .turnTo(Math.toRadians((270)))
-                // Start of the cycling
-                .setTangent(Math.toRadians(335))
-                .splineToLinearHeading(this.RUNGS1, Math.toRadians(0))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.SCORE, Math.toRadians(150))
-                .setTangent(Math.toRadians(315))
-                .splineToLinearHeading(this.RUNGS2, Math.toRadians(0))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.SCORE, Math.toRadians(150))
-                .setTangent(Math.toRadians(315))
-                .splineToLinearHeading(this.RUNGS3, Math.toRadians(0))
-                .build();
-    }
-    public Action park(TrajectoryActionBuilder builder){
-        return builder.setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.PARK, Math.toRadians(90))
-                .build();
-    }
-
-    public Pose2d getStart() {
-        return this.START;
-    }
-}
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarPush.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarPush.java
deleted file mode 100644
index bb94d18..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarPush.java
+++ /dev/null
@@ -1,94 +0,0 @@
-package com.example.meepmeep.trajectories;
-
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-import com.acmerobotics.roadrunner.Vector2d;
-
-import org.json.JSONObject;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.Scanner;
-
-public class BlueFarPush {
-    private final Pose2d START, PUSH1, PUSH2, PUSH3, SCOREPUSH, PARK;
-
-    public BlueFarPush(int type) {
-        String jsonString = "";
-
-        try {
-            File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json" : "/sdcard/FIRST/positions/blue/far-basket.json"));
-            Scanner reader = new Scanner(file);
-
-            while (reader.hasNextLine()) {
-                jsonString += reader.nextLine();
-            }
-        } catch (
-                FileNotFoundException e) {
-            e.printStackTrace();
-        }
-
-        JSONObject positions = new JSONObject(jsonString);
-
-        this.START = new Pose2d(
-                positions.getJSONObject("START").getDouble("x"),
-                positions.getJSONObject("START").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("START").getDouble("heading"))
-        );
-
-        this.PUSH1 = new Pose2d(
-                positions.getJSONObject("PUSH1").getDouble("x"),
-                positions.getJSONObject("PUSH1").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PUSH1").getDouble("heading"))
-        );
-
-        this.PUSH2 = new Pose2d(
-                positions.getJSONObject("PUSH2").getDouble("x"),
-                positions.getJSONObject("PUSH2").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PUSH2").getDouble("heading"))
-        );
-
-        this.PUSH3 = new Pose2d(
-                positions.getJSONObject("PUSH3").getDouble("x"),
-                positions.getJSONObject("PUSH3").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PUSH3").getDouble("heading"))
-        );
-
-        this.SCOREPUSH = new Pose2d(
-                positions.getJSONObject("SCOREPUSH").getDouble("x"),
-                positions.getJSONObject("SCOREPUSH").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SCOREPUSH").getDouble("heading"))
-        );
-
-        this.PARK = new Pose2d(
-                positions.getJSONObject("PARK").getDouble("x"),
-                positions.getJSONObject("PARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PARK").getDouble("heading"))
-        );
-    }
-
-    public Action start(TrajectoryActionBuilder builder) {
-        return builder.splineToLinearHeading(this.SCOREPUSH, Math.toRadians((270)))
-                .turnTo(Math.toRadians(270))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.PUSH1, Math.toRadians(180))
-                .strafeTo(new Vector2d(-46, 55))
-                .setTangent(Math.toRadians(270))
-                .splineToLinearHeading(this.PUSH2, Math.toRadians(180))
-                .strafeTo(new Vector2d(-56, 55))
-                .setTangent(Math.toRadians(270))
-                .splineToLinearHeading(this.PUSH3, Math.toRadians(180))
-                .strafeTo(new Vector2d(-62, 55))
-                .build();
-    }
-    public Action park(TrajectoryActionBuilder builder){
-        return builder.setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.PARK, Math.toRadians(90))
-                .build();
-    }
-
-    public Pose2d getStart() {
-        return this.START;
-    }
-}
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java
deleted file mode 100644
index 6f9bed1..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java
+++ /dev/null
@@ -1,115 +0,0 @@
-package com.example.meepmeep.trajectories;
-
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-
-import org.json.JSONObject;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.Scanner;
-
-public class RedCloseBasket {
-    private final Pose2d START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, SUBMERSIBLE, PARK;
-
-    public RedCloseBasket(int type) {
-        String jsonString = "";
-
-        try {
-            File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json" : "/sdcard/FIRST/positions/red/far-basket.json"));
-            Scanner reader = new Scanner(file);
-
-            while (reader.hasNextLine()) {
-                jsonString += reader.nextLine();
-            }
-        } catch (
-                FileNotFoundException e) {
-            e.printStackTrace();
-        }
-
-        JSONObject positions = new JSONObject(jsonString);
-
-        this.START = new Pose2d(
-                positions.getJSONObject("START").getDouble("x"),
-                positions.getJSONObject("START").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("START").getDouble("heading"))
-        );
-
-        this.RUNGS = new Pose2d(
-                positions.getJSONObject("RUNG").getDouble("x"),
-                positions.getJSONObject("RUNG").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading"))
-        );
-
-        this.LEFT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.MID_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.RIGHT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.SCORE = new Pose2d(
-                positions.getJSONObject("SCORE").getDouble("x"),
-                positions.getJSONObject("SCORE").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading"))
-        );
-
-        this.SUBMERSIBLE = new Pose2d(
-                positions.getJSONObject("SUBMERSIBLE").getDouble("x"),
-                positions.getJSONObject("SUBMERSIBLE").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SUBMERSIBLE").getDouble("heading"))
-        );
-
-        this.PARK = new Pose2d(
-                positions.getJSONObject("PARK").getDouble("x"),
-                positions.getJSONObject("PARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PARK").getDouble("heading"))
-        );
-
-    }
-
-    public Action start(TrajectoryActionBuilder builder) {
-        return builder.splineToLinearHeading(this.RUNGS, Math.toRadians(0))
-                .setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.MID_SPIKEMARK, Math.toRadians(210))
-                .setTangent(0)
-                .splineToLinearHeading(this.SCORE, Math.toRadians(273))
-                .splineToLinearHeading(this.LEFT_SPIKEMARK, Math.toRadians(180))
-                .setTangent(0)
-                .splineToLinearHeading(this.SCORE, Math.toRadians(273))
-                .splineToLinearHeading(this.RIGHT_SPIKEMARK, Math.toRadians(180))
-                .setTangent(0)
-                .splineToLinearHeading(this.SCORE, Math.toRadians(273))
-                .build();
-    }
-
-    public Action cycle(TrajectoryActionBuilder builder) {
-        return builder.setTangent(Math.toRadians(0))
-                .splineToLinearHeading(this.SUBMERSIBLE, Math.toRadians(90))
-                .setTangent(Math.toRadians(270))
-                .splineToLinearHeading(this.SCORE, Math.toRadians(180))
-                .build();
-    }
-
-    public Action park(TrajectoryActionBuilder builder){
-        return builder.setTangent(0)
-                .splineToLinearHeading(PARK, Math.toRadians(0))
-                .build();
-    }
-
-    public Pose2d getStart() {
-        return this.START;
-    }
-}
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java
deleted file mode 100644
index 89eee83..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java
+++ /dev/null
@@ -1,124 +0,0 @@
-package com.example.meepmeep.trajectories;
-
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-
-import org.json.JSONObject;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.Scanner;
-
-public class RedFarBasket {
-    private final Pose2d START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK;
-
-    public RedFarBasket(int type) {
-        String jsonString = "";
-
-        try {
-            File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json" : "/sdcard/FIRST/positions/red/far-basket.json"));
-            Scanner reader = new Scanner(file);
-
-            while (reader.hasNextLine()) {
-                jsonString += reader.nextLine();
-            }
-        } catch (
-                FileNotFoundException e) {
-            e.printStackTrace();
-        }
-
-        JSONObject positions = new JSONObject(jsonString);
-
-        this.START = new Pose2d(
-                positions.getJSONObject("START").getDouble("x"),
-                positions.getJSONObject("START").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("START").getDouble("heading"))
-        );
-
-        this.RUNGS = new Pose2d(
-                positions.getJSONObject("RUNG").getDouble("x"),
-                positions.getJSONObject("RUNG").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading"))
-        );
-
-        this.RUNGS1 = new Pose2d(
-                positions.getJSONObject("RUNG1").getDouble("x"),
-                positions.getJSONObject("RUNG1").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG1").getDouble("heading"))
-        );
-
-        this.RUNGS2 = new Pose2d(
-                positions.getJSONObject("RUNG2").getDouble("x"),
-                positions.getJSONObject("RUNG2").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG2").getDouble("heading"))
-        );
-
-        this.RUNGS3 = new Pose2d(
-                positions.getJSONObject("RUNG3").getDouble("x"),
-                positions.getJSONObject("RUNG3").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RUNG3").getDouble("heading"))
-        );
-
-        this.LEFT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("LEFT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.MID_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("MID_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("MID_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.RIGHT_SPIKEMARK = new Pose2d(
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("x"),
-                positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading"))
-        );
-
-        this.SCORE = new Pose2d(
-                positions.getJSONObject("SCORE").getDouble("x"),
-                positions.getJSONObject("SCORE").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading"))
-        );
-
-        this.PARK = new Pose2d(
-                positions.getJSONObject("PARK").getDouble("x"),
-                positions.getJSONObject("PARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PARK").getDouble("heading"))
-        );
-    }
-
-    public Action start(TrajectoryActionBuilder builder) {
-        return builder.splineToLinearHeading(this.RUNGS, Math.toRadians(180))
-                .setTangent(Math.toRadians(0))
-                .splineToLinearHeading(this.MID_SPIKEMARK, Math.toRadians(330))
-                .turnTo(this.LEFT_SPIKEMARK.heading)
-                .turnTo(Math.toRadians(270))
-                .turnTo(this.RIGHT_SPIKEMARK.heading)
-                .turnTo(Math.toRadians(270))
-                .turnTo(Math.toRadians(90))
-                .setTangent(Math.toRadians(155))
-                .splineToLinearHeading(this.RUNGS1, Math.toRadians(180))
-                .setTangent(Math.toRadians(0))
-                .splineToLinearHeading(this.SCORE, Math.toRadians(330))
-                .setTangent(Math.toRadians(135))
-                .splineToLinearHeading(this.RUNGS2, Math.toRadians(180))
-                .setTangent(Math.toRadians(0))
-                .splineToLinearHeading(this.SCORE, Math.toRadians(330))
-                .setTangent(Math.toRadians(135))
-                .splineToLinearHeading(this.RUNGS3, Math.toRadians(180))
-                .build();
-    }
-
-    public Action park(TrajectoryActionBuilder builder){
-        return builder.setTangent(Math.toRadians(0))
-                .splineToLinearHeading(this.PARK, Math.toRadians(270)).build();
-    }
-
-    public Pose2d getStart() {
-        return this.START;
-    }
-}
diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarPush.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarPush.java
deleted file mode 100644
index c8a1ee8..0000000
--- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarPush.java
+++ /dev/null
@@ -1,94 +0,0 @@
-package com.example.meepmeep.trajectories;
-
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-import com.acmerobotics.roadrunner.Vector2d;
-
-import org.json.JSONObject;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.Scanner;
-
-public class RedFarPush {
-    private final Pose2d START, PUSH1, PUSH2, PUSH3, SCOREPUSH, PARK;
-
-    public RedFarPush(int type) {
-        String jsonString = "";
-
-        try {
-            File file = new File(new File("").getAbsolutePath().concat(type == 0 ? "/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json" : "/sdcard/FIRST/positions/blue/far-basket.json"));
-            Scanner reader = new Scanner(file);
-
-            while (reader.hasNextLine()) {
-                jsonString += reader.nextLine();
-            }
-        } catch (
-                FileNotFoundException e) {
-            e.printStackTrace();
-        }
-
-        JSONObject positions = new JSONObject(jsonString);
-
-        this.START = new Pose2d(
-                positions.getJSONObject("START").getDouble("x"),
-                positions.getJSONObject("START").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("START").getDouble("heading"))
-        );
-
-        this.PUSH1 = new Pose2d(
-                positions.getJSONObject("PUSH1").getDouble("x"),
-                positions.getJSONObject("PUSH1").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PUSH1").getDouble("heading"))
-        );
-
-        this.PUSH2 = new Pose2d(
-                positions.getJSONObject("PUSH2").getDouble("x"),
-                positions.getJSONObject("PUSH2").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PUSH2").getDouble("heading"))
-        );
-
-        this.PUSH3 = new Pose2d(
-                positions.getJSONObject("PUSH3").getDouble("x"),
-                positions.getJSONObject("PUSH3").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PUSH3").getDouble("heading"))
-        );
-
-        this.SCOREPUSH = new Pose2d(
-                positions.getJSONObject("SCOREPUSH").getDouble("x"),
-                positions.getJSONObject("SCOREPUSH").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("SCOREPUSH").getDouble("heading"))
-        );
-
-        this.PARK = new Pose2d(
-                positions.getJSONObject("PARK").getDouble("x"),
-                positions.getJSONObject("PARK").getDouble("y"),
-                Math.toRadians(positions.getJSONObject("PARK").getDouble("heading"))
-        );
-    }
-
-    public Action start(TrajectoryActionBuilder builder) {
-        return builder.splineToLinearHeading(this.SCOREPUSH, Math.toRadians((90)))
-                .turnTo(Math.toRadians(270))
-                .setTangent(Math.toRadians(0))
-                .splineToLinearHeading(this.PUSH1, Math.toRadians(0))
-                .strafeTo(new Vector2d(46, -55))
-                .setTangent(Math.toRadians(90))
-                .splineToLinearHeading(this.PUSH2, Math.toRadians(0))
-                .strafeTo(new Vector2d(56, -55))
-                .setTangent(Math.toRadians(90))
-                .splineToLinearHeading(this.PUSH3, Math.toRadians(0))
-                .strafeTo(new Vector2d(62, -55))
-                .build();
-    }
-    public Action park(TrajectoryActionBuilder builder){
-        return builder.setTangent(Math.toRadians(180))
-                .splineToLinearHeading(this.PARK, Math.toRadians(90))
-                .build();
-    }
-
-    public Pose2d getStart() {
-        return this.START;
-    }
-}
diff --git a/Pipelines/BluePipeline.vpr b/Pipelines/BluePipeline.vpr
new file mode 100644
index 0000000..1f31910
--- /dev/null
+++ b/Pipelines/BluePipeline.vpr
@@ -0,0 +1 @@
+{"area_max":100.0,"area_min":0.001,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":1500.0,"botfloorsnap":0,"botlength":0.4572,"bottype":"swerve","botwidth":0.4572,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"cpu","contour_grouping":0,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":10.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"desc":"BluePipeline","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"cpu","dilation_steps":10,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":4,"exposure":3300.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"","fiducial_locfilters":"","fiducial_qualitythreshold":2.0,"fiducial_resdiv":2,"fiducial_size":101.6,"fiducial_skip3d":1,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dtargposebotspace","flicker":0,"force_convex":1,"hue_max":127,"hue_min":79,"image_flip":0,"image_source":0,"img_to_show":1,"intersection_filter":0,"invert_hue":0,"lcgain":45.0,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":1,"pipeline_led_power":100,"pipeline_res":1,"pipeline_type":"pipe_color","python_snapscript_name":"","quality_focus":0.3,"red_balance":1280.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.0,"rspitch":0.0,"rsroll":0.0,"rss":0.0,"rsu":0.0,"rsyaw":0.0,"sat_max":255,"sat_min":207,"send_corners":0,"send_json":0,"tsf":0.0,"tss":0.0,"tsu":0.0,"val_max":255,"val_min":76,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0}
\ No newline at end of file
diff --git a/Pipelines/RedPipeline.vpr b/Pipelines/RedPipeline.vpr
new file mode 100644
index 0000000..27217f8
--- /dev/null
+++ b/Pipelines/RedPipeline.vpr
@@ -0,0 +1 @@
+{"area_max":100.0,"area_min":0.029449992100000006,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":1500.0,"botfloorsnap":0,"botlength":0.4572,"bottype":"swerve","botwidth":0.4572,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"cpu","contour_grouping":0,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":0.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"desc":"RedPipeline","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"cpu","dilation_steps":2,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":0,"exposure":3300.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"","fiducial_locfilters":"","fiducial_qualitythreshold":2.0,"fiducial_resdiv":2,"fiducial_size":101.6,"fiducial_skip3d":1,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dtargposebotspace","flicker":0,"force_convex":1,"hue_max":179,"hue_min":0,"image_flip":0,"image_source":0,"img_to_show":1,"intersection_filter":0,"invert_hue":1,"lcgain":45.0,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":1,"pipeline_led_power":100,"pipeline_res":1,"pipeline_type":"pipe_color","python_snapscript_name":"","quality_focus":0.3,"red_balance":1280.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.0,"rspitch":0.0,"rsroll":0.0,"rss":0.0,"rsu":0.0,"rsyaw":0.0,"sat_max":255,"sat_min":247,"send_corners":0,"send_json":0,"tsf":0.0,"tss":0.0,"tsu":0.0,"val_max":255,"val_min":52,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0}
\ No newline at end of file
diff --git a/Pipelines/YellowPipeline.vpr b/Pipelines/YellowPipeline.vpr
new file mode 100644
index 0000000..559b74e
--- /dev/null
+++ b/Pipelines/YellowPipeline.vpr
@@ -0,0 +1 @@
+{"area_max":100.0,"area_min":0.001,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":1500.0,"botfloorsnap":0,"botlength":0.4572,"bottype":"swerve","botwidth":0.4572,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"cpu","contour_grouping":0,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":10.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"desc":"YellowPipeline","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"cpu","dilation_steps":10,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":1,"exposure":3283.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"","fiducial_locfilters":"","fiducial_qualitythreshold":2.0,"fiducial_resdiv":2,"fiducial_size":101.6,"fiducial_skip3d":1,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dtargposebotspace","flicker":0,"force_convex":1,"hue_max":30,"hue_min":21,"image_flip":0,"image_source":0,"img_to_show":1,"intersection_filter":0,"invert_hue":0,"lcgain":45.0,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":1,"pipeline_led_power":100,"pipeline_res":1,"pipeline_type":"pipe_color","python_snapscript_name":"","quality_focus":0.3,"red_balance":1280.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.0,"rspitch":0.0,"rsroll":0.0,"rss":0.0,"rsu":0.0,"rsyaw":0.0,"sat_max":255,"sat_min":244,"send_corners":0,"send_json":0,"tsf":0.0,"tss":0.0,"tsu":0.0,"val_max":247,"val_min":162,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0}
\ No newline at end of file
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 511928c..0d6eaa4 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -33,25 +33,12 @@ android {
 
 repositories {
     maven { url = 'https://jitpack.io' }
-    maven { url = 'https://maven.brott.dev/' }
-    maven {
-        url = 'https://repo.dairy.foundation/releases'
-    }
 }
 
 dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
 
-    // IMPORT PATHING VARS
-    implementation project(path: ':PathVisualizer')
-
-    // FTCLIB
     implementation 'org.ftclib.ftclib:core:2.0.1'
-
-    // ROADRUNNER V1
-    implementation "page.j5155.roadrunner:ftc-otos:0.1.2+0.1.14"
-    implementation "com.acmerobotics.roadrunner:core:1.0.0"
-    implementation "com.acmerobotics.roadrunner:actions:1.0.0"
-    implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
+    implementation 'org.json:json:20240303'
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java
index fa40095..9122e2f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java
@@ -1,242 +1,322 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.command.Command;
+import com.arcrobotics.ftclib.command.ConditionalCommand;
 import com.arcrobotics.ftclib.command.SequentialCommandGroup;
 import com.arcrobotics.ftclib.command.WaitCommand;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.localization.Pose;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotAccepting;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotScore;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateDown;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateReady;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateUp;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmAccepting;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmBasket;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmDriveIn;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmIntake;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmSpecimen;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmSweep;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawClose;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawOpen;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoAccepting;
+import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoBasket;
+import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoDriveIn;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoReady;
-import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
+import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoSpecimen;
+import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoSweep;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftAccepting;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftDecrement;
-import org.firstinspires.ftc.teamcode.commands.lift.LiftHighBasket;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftDriveIn;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftHighRung;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftIncrement;
-import org.firstinspires.ftc.teamcode.commands.lift.LiftLowBasket;
-import org.firstinspires.ftc.teamcode.commands.lift.LiftLowRung;
-import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftSlam;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotAccepting;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotBasket;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotDriveIn;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotIntake;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotReady;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotSpecimen;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotSweep;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristAccepting;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristBasket;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristLeft;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristReady;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristRight;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristSpecimen;
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
 import org.firstinspires.ftc.teamcode.subsystems.Extendo;
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
 import org.firstinspires.ftc.teamcode.utils.commands.GamepadTrigger;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
 @Config
 public class CommandRobot {
-    public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, slamdown, specimen;
-    public GamepadTrigger intakeAccept, intakeReject;
-
     private TeleOpMode mode;
 
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
     private final Extendo extendo;
     private final Claw claw;
+    private final Pivot pivot;
+    private final Wrist wrist;
+    private final Arm arm;
 
-    private Drivetrain drivetrain;
+    private final Drivetrain drivetrain;
 
     private GamepadEx gamepad1;
     private GamepadEx gamepad2;
-    private final OpModeCore opMode;
 
-    public static int CLAW_ACCEPT_DELAY = 450;
-    public static int CLAW_RETRACT_DELAY = 450;
-    public static int LIFT_DELAY = 250;
+    private boolean intakeToggle;
+
+    public static int ACCEPTING_WAIT = 700;
+    public static int READY_WAIT = 700;
+
+    private GamepadTrigger lt, rt;
 
-    public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad gamepad1, Gamepad gamepad2, OpModeCore opMode, TeleOpMode mode) {
-        this.telemetry = telemetry;
+    public CommandRobot(HardwareMap hwMap, Gamepad gamepad1, Gamepad gamepad2, TeleOpMode mode) {
+        this.drivetrain = new Drivetrain(hwMap, new Pose(0, 0, 0));
+        this.lift = new Lift(hwMap);
+        this.extendo = new Extendo(hwMap);
+        this.claw = new Claw(hwMap);
+        this.pivot = new Pivot(hwMap);
+        this.wrist = new Wrist(hwMap);
+        this.arm = new Arm(hwMap);
 
-        this.drivetrain = new Drivetrain(hwMap, telemetry, new Pose2d(0, 0, 0));
-        this.lift = new Lift(hwMap, telemetry);
-        this.extendo = new Extendo(hwMap, telemetry);
-        this.claw = new Claw(hwMap, telemetry);
+        this.intakeToggle = true;
 
         this.gamepad1 = new GamepadEx(gamepad1);
         this.gamepad2 = new GamepadEx(gamepad2);
 
-        this.opMode = opMode;
         this.mode = mode;
 
-        this.configureCommands();
+        this.lt = new GamepadTrigger(GamepadKeys.Trigger.LEFT_TRIGGER, p -> this.extendo.setPower(p / 2), this.gamepad1);
+        this.rt = new GamepadTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER, p -> this.extendo.setPower(-p / 2), this.gamepad1);
+
         this.configureControls();
     }
 
-    public CommandRobot(HardwareMap hwMap, Pose2d startPose, MultipleTelemetry telemetry, OpModeCore opMode) {
-        this.telemetry = telemetry;
+    public CommandRobot(HardwareMap hwMap, Pose startPose) {
+        this.lift = new Lift(hwMap);
+        this.extendo = new Extendo(hwMap);
+        this.claw = new Claw(hwMap);
+        this.pivot = new Pivot(hwMap);
+        this.wrist = new Wrist(hwMap);
+        this.arm = new Arm(hwMap);
 
-        this.lift = new Lift(hwMap, telemetry);
-        this.extendo = new Extendo(hwMap, telemetry);
-        this.claw = new Claw(hwMap, telemetry);
+        this.drivetrain = new Drivetrain(hwMap, startPose);
+    }
 
-        this.opMode = opMode;
+    public void startThreads(OpModeCore opMode) {
+        this.drivetrain.startThread(this.gamepad1, opMode);
+        this.extendo.startThread(opMode);
+        this.lift.startThread(opMode);
+    }
 
-        this.configureCommands();
+    public void startAutoThreads(OpModeCore opMode) {
+        this.extendo.startThread(opMode);
+        this.lift.startThread(opMode);
     }
 
-    public MecanumDrive getDrive() {
-        return this.drivetrain.getDrive();
+    public void configureControls() {
+        switch (this.mode) {
+            case KELLY:
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
+                        .whenPressed(new ConditionalCommand(this.ready(), this.accepting(), () -> {
+                            if (this.lift.getPosition() > 100 || this.arm.getPosition() > 0.93)
+                                this.intakeToggle = true;
+
+                            else this.intakeToggle = !this.intakeToggle;
+                            return this.intakeToggle;
+                        }));
+
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
+                        .whenPressed(this.liftDecrement());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_UP)
+                        .whenPressed(this.liftIncrement());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)
+                        .whenPressed(this.wristRight());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
+                        .whenPressed(this.wristLeft());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.Y)
+                        .whenPressed(this.driveIn());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
+                        .whenPressed(this.specimen());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
+                        .whenPressed(new ConditionalCommand(this.close(), this.intake(), () -> this.intakeToggle));
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
+                        .whenPressed(this.open());
+                break;
+
+            case DEV:
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
+                        .whenPressed(new ConditionalCommand(this.ready(), this.accepting(), () -> {
+                            if (this.lift.getPosition() > 100 || this.arm.getPosition() > 0.93)
+                                this.intakeToggle = true;
+
+                            else this.intakeToggle = !this.intakeToggle;
+                            return this.intakeToggle;
+                        }));
+
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
+                        .whenPressed(this.liftDecrement());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_UP)
+                        .whenPressed(this.liftIncrement());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)
+                        .whenPressed(this.wristRight());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
+                        .whenPressed(this.sweep());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.Y)
+                        .whenPressed(this.highRung());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
+                        .whenPressed(this.specimen());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.X)
+                        .whenPressed(this.slam());
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
+                        .whenPressed(new ConditionalCommand(this.close(), this.intake(), () -> this.intakeToggle));
+                this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
+                        .whenPressed(this.open());
+                break;
+        }
     }
 
-    public void startThreads() {
-        if (this.mode == TeleOpMode.OWEN || this.mode == TeleOpMode.RYAN)
-            this.drivetrain.startThread(this.gamepad1, this.opMode);
-        else
-            this.drivetrain.startThread(this.gamepad2, this.opMode);
+    public enum TeleOpMode {
+        KELLY,
+        DEV
+    }
 
-        this.lift.startThread(this.opMode);
+    public Follower getFollower() {
+        return this.drivetrain.getFollower();
     }
 
-    public void configureCommands() {
-        this.ready = new SequentialCommandGroup(
-                new LiftAccepting(this.telemetry, this.lift),
-                new ClawRotateReady(this.telemetry, this.claw),
-                new ClawPivotAccepting(this.telemetry, this.claw),
-                new WaitCommand(CommandRobot.CLAW_RETRACT_DELAY),
-                new ExtendoReady(this.telemetry, this.extendo)
+    public Command ready() {
+        return new SequentialCommandGroup(
+                new ClawClose(this.claw),
+                new WristReady(this.wrist),
+                new PivotReady(this.pivot),
+                new ExtendoReady(this.extendo),
+                new ArmSpecimen(this.arm),
+                new LiftAccepting(this.lift)
         );
+    }
 
-        this.accepting = new SequentialCommandGroup(
-                new LiftAccepting(this.telemetry, this.lift),
-                new ExtendoAccepting(this.telemetry, this.extendo),
-                new WaitCommand(CommandRobot.CLAW_ACCEPT_DELAY),
-                new ClawRotateDown(this.telemetry, this.claw),
-                new ClawPivotAccepting(this.telemetry, this.claw)
+    public Command specimen() {
+        return new SequentialCommandGroup(
+                new LiftAccepting(this.lift),
+                new WristBasket(this.wrist),
+                new PivotBasket(this.pivot),
+                new ArmBasket(this.arm),
+                new ExtendoBasket(this.extendo),
+                new WaitCommand(400),
+                new ClawOpen(this.claw)
         );
+    }
 
-        this.highBasket = new SequentialCommandGroup(
-                new LiftHighBasket(this.telemetry, this.lift),
-                new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw),
-                new ClawRotateUp(this.telemetry, this.claw)
+    public Command accepting() {
+        return new SequentialCommandGroup(
+                new LiftAccepting(this.lift),
+                new ExtendoAccepting(this.extendo),
+                new WaitCommand(ACCEPTING_WAIT),
+                new ClawOpen(this.claw),
+                new ArmAccepting(this.arm),
+                new PivotAccepting(this.pivot),
+                new WristAccepting(this.wrist)
         );
+    }
 
-        this.lowBasket = new SequentialCommandGroup(
-                new LiftLowBasket(this.telemetry, this.lift),
-                new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw),
-                new ClawRotateUp(this.telemetry, this.claw)
+    public Command highRung() {
+        return new SequentialCommandGroup(
+                new ClawClose(this.claw),
+                new LiftHighRung(this.lift),
+                new ExtendoSpecimen(this.extendo),
+                new WaitCommand(350),
+                new PivotSpecimen(this.pivot),
+                new WristSpecimen(this.wrist),
+                new ArmSpecimen(this.arm)
         );
+    }
 
-        this.highRung = new SequentialCommandGroup(
-                new LiftHighRung(this.telemetry, this.lift),
-                new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw),
-                new ClawRotateUp(this.telemetry, this.claw)
+    public Command intake() {
+        return new SequentialCommandGroup(
+                new ClawOpen(this.claw),
+                new WaitCommand(100),
+                new ArmIntake(this.arm),
+                new PivotIntake(this.pivot),
+                new WaitCommand(200),
+                new ClawClose(this.claw),
+                new WaitCommand(200),
+                new PivotAccepting(this.pivot),
+                new ArmAccepting(this.arm)
         );
+    }
 
-        this.lowRung = new SequentialCommandGroup(
-                new LiftLowRung(this.telemetry, this.lift),
-                new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw),
-                new ClawRotateUp(this.telemetry, this.claw)
-
+    public Command slam() {
+        return new SequentialCommandGroup(
+                new LiftSlam(this.lift),
+                new WaitCommand(500),
+                new ClawOpen(this.claw)
         );
+    }
 
-        this.specimen = new SequentialCommandGroup(
-                new LiftLowRung(this.telemetry, this.lift),
-                new ExtendoReady(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw),
-                new ClawRotateReady(this.telemetry, this.claw)
+    public Command driveIn() {
+        return new SequentialCommandGroup(
+                new ClawClose(this.claw),
+                new LiftDriveIn(this.lift),
+                new ExtendoDriveIn(this.extendo),
+                new PivotDriveIn(this.pivot),
+                new WristSpecimen(this.wrist),
+                new ArmDriveIn(this.arm)
         );
+    }
 
-        this.intakeAccept = new GamepadTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER, d -> this.claw.setClawPower(-d), this.gamepad2);
-        this.intakeReject = new GamepadTrigger(GamepadKeys.Trigger.LEFT_TRIGGER, this.claw::setClawPower, this.gamepad2);
-
-        this.liftIncrement = new LiftIncrement(this.telemetry, this.lift);
+    public Command sweep() {
+        return new SequentialCommandGroup(
+                new LiftAccepting(this.lift),
+                new ClawClose(this.claw),
+                new PivotSweep(this.pivot),
+                new ArmSweep(this.arm),
+                new ExtendoSweep(this.extendo)
+        );
+    }
 
-        this.liftDecrement = new LiftDecrement(this.telemetry, this.lift);
+    public Command close() {
+        return new ClawClose(this.claw);
+    }
 
+    public Command open() {
+        return new ClawOpen(this.claw);
     }
 
-    public void configureControls() {
-        switch (this.mode) {
-            /* ------------------------------------- */
-            /* --------------- OWEN ---------------- */
-            /* ------------------------------------- */
+    public Command liftIncrement() {
+        return new LiftIncrement(this.lift);
+    }
 
-            case OWEN:
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
-                        .whenPressed(this.ready);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
-                        .whenPressed(this.accepting);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_UP)
-                        .whenPressed(this.liftIncrement);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
-                        .whenPressed(this.liftDecrement);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.X)
-                        .whenPressed(this.lowRung);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.Y)
-                        .whenPressed(this.highRung);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)
-                        .whenPressed(this.highBasket);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
-                        .whenPressed(this.lowBasket);
-                break;
+    public Command liftDecrement() {
+        return new LiftDecrement(this.lift);
+    }
 
-            /* ------------------------------------- */
-            /* --------------- RYAN ---------------- */
-            /* ------------------------------------- */
+    public Command wristRight() {
+        return new WristRight(this.wrist);
+    }
 
-            case RYAN:
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
-                        .whenPressed(this.accepting);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.Y)
-                        .whenPressed(this.highBasket);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
-                        .whenPressed(this.ready);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.X)
-                        .whenPressed(this.lowBasket);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_UP)
-                        .whenPressed(this.liftIncrement);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
-                        .whenPressed(this.liftDecrement);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.X)
-                        .whenPressed(this.lowRung);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
-                        .whenPressed(this.highRung);
-                break;
+    public Command wristLeft() {
+        return new WristLeft(this.wrist);
+    }
 
-            /* ------------------------------------- */
-            /* ------------ RYAN & KELLY ----------- */
-            /* ------------------------------------- */
+    public void update() {
+        this.rt.update();
+        this.lt.update();
 
-            case RYAN_KELLY:
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
-                        .whenPressed(this.accepting);
-                this.gamepad2.getGamepadButton(GamepadKeys.Button.Y)
-                        .whenPressed(this.highBasket);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
-                        .whenPressed(this.ready);
-                this.gamepad2.getGamepadButton(GamepadKeys.Button.A)
-                        .whenPressed(this.lowBasket);
-                this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_UP)
-                        .whenPressed(this.liftIncrement);
-                this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
-                        .whenPressed(this.liftDecrement);
-                this.gamepad2.getGamepadButton(GamepadKeys.Button.X)
-                        .whenPressed(this.lowRung);
-                this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
-                        .whenPressed(this.highRung);
-                break;
+        if (this.extendo.getPosition() > 100) {
+            Drivetrain.MAX_ANGULAR_VEL = 0.4;
+            Drivetrain.MAX_ANGULAR_ACCEL = 0.1;
+        } else {
+            Drivetrain.MAX_ANGULAR_ACCEL = 0.2;
+            Drivetrain.MAX_ANGULAR_VEL = 0.6;
         }
     }
-
-    public enum TeleOpMode {
-        OWEN,
-        RYAN,
-        RYAN_KELLY
-    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmAccepting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmAccepting.java
new file mode 100644
index 0000000..cd4b3e1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmAccepting.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.arm;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+
+public class ArmAccepting extends CommandBase {
+    private final Arm arm;
+
+    public ArmAccepting(final Arm arm) {
+        this.arm = arm;
+        super.addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        this.arm.setPosition(Arm.ACCEPTING);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmBasket.java
new file mode 100644
index 0000000..a666cb1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmBasket.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.arm;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+
+public class ArmBasket extends CommandBase {
+    private final Arm arm;
+
+    public ArmBasket(final Arm arm) {
+        this.arm = arm;
+        super.addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        this.arm.setPosition(Arm.SCORE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmDriveIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmDriveIn.java
new file mode 100644
index 0000000..c3b3903
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmDriveIn.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.arm;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+
+public class ArmDriveIn extends CommandBase {
+    private final Arm arm;
+
+    public ArmDriveIn(final Arm arm) {
+        this.arm = arm;
+        super.addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        this.arm.setPosition(Arm.DRIVE_IN);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmIntake.java
new file mode 100644
index 0000000..f60c2e5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmIntake.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.arm;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+
+public class ArmIntake extends CommandBase {
+    private final Arm arm;
+
+    public ArmIntake(final Arm arm) {
+        this.arm = arm;
+        super.addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        this.arm.setPosition(Arm.INTAKE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmReady.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmReady.java
new file mode 100644
index 0000000..89c073e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmReady.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.arm;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+
+public class ArmReady extends CommandBase {
+    private final Arm arm;
+
+    public ArmReady(final Arm arm) {
+        this.arm = arm;
+        super.addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        this.arm.setPosition(Arm.READY);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmSpecimen.java
new file mode 100644
index 0000000..6bfc18a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmSpecimen.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.arm;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+
+public class ArmSpecimen extends CommandBase {
+    private final Arm arm;
+
+    public ArmSpecimen(final Arm arm) {
+        this.arm = arm;
+        super.addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        this.arm.setPosition(Arm.SPECIMEN);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmSweep.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmSweep.java
new file mode 100644
index 0000000..4fb7263
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmSweep.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.arm;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+
+public class ArmSweep extends CommandBase {
+    private final Arm arm;
+
+    public ArmSweep(final Arm arm) {
+        this.arm = arm;
+        super.addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        this.arm.setPosition(Arm.SWEEP);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateUp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawClose.java
similarity index 55%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateUp.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawClose.java
index 284a146..18ac32f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateUp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawClose.java
@@ -1,24 +1,20 @@
 package org.firstinspires.ftc.teamcode.commands.claw;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 
-public class ClawRotateUp extends CommandBase {
-    private final MultipleTelemetry telemetry;
+public class ClawClose extends CommandBase {
     private final Claw claw;
 
-    public ClawRotateUp(final MultipleTelemetry telemetry, final Claw claw) {
-        this.telemetry = telemetry;
-
+    public ClawClose(final Claw claw) {
         this.claw = claw;
         super.addRequirements(claw);
     }
 
     @Override
     public void initialize() {
-        this.claw.setPivotPosition(Claw.UP);
+        this.claw.setPosition(Claw.CLOSE);
     }
 
     @Override
@@ -26,3 +22,4 @@ public boolean isFinished() {
         return true;
     }
 }
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateDown.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawOpen.java
similarity index 54%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateDown.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawOpen.java
index 32c63e4..25353c9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateDown.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawOpen.java
@@ -1,24 +1,20 @@
 package org.firstinspires.ftc.teamcode.commands.claw;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 
-public class ClawRotateDown extends CommandBase {
-    private final MultipleTelemetry telemetry;
+public class ClawOpen extends CommandBase {
     private final Claw claw;
 
-    public ClawRotateDown(final MultipleTelemetry telemetry, final Claw claw) {
-        this.telemetry = telemetry;
-
+    public ClawOpen(final Claw claw) {
         this.claw = claw;
         super.addRequirements(claw);
     }
 
     @Override
     public void initialize() {
-        this.claw.setPivotPosition(Claw.DOWN);
+        this.claw.setPosition(Claw.OPEN);
     }
 
     @Override
@@ -26,4 +22,3 @@ public boolean isFinished() {
         return true;
     }
 }
-
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotAccepting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotAccepting.java
deleted file mode 100644
index 53d993c..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotAccepting.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package org.firstinspires.ftc.teamcode.commands.claw;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.command.CommandBase;
-
-import org.firstinspires.ftc.teamcode.subsystems.Claw;
-
-public class ClawPivotAccepting extends CommandBase {
-    private final MultipleTelemetry telemetry;
-    private final Claw claw;
-
-    public ClawPivotAccepting(final MultipleTelemetry telemetry, final Claw claw) {
-        this.telemetry = telemetry;
-
-        this.claw = claw;
-        super.addRequirements(claw);
-    }
-
-    @Override
-    public void initialize() {
-        this.claw.setPivotPosition(Claw.ACCEPTING);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return true;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotScore.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotScore.java
deleted file mode 100644
index efb8dac..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotScore.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package org.firstinspires.ftc.teamcode.commands.claw;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.command.CommandBase;
-
-import org.firstinspires.ftc.teamcode.subsystems.Claw;
-
-public class ClawPivotScore extends CommandBase {
-    private final MultipleTelemetry telemetry;
-    private final Claw claw;
-
-    public ClawPivotScore(final MultipleTelemetry telemetry, final Claw claw) {
-        this.telemetry = telemetry;
-
-        this.claw = claw;
-        super.addRequirements(claw);
-    }
-
-    @Override
-    public void initialize() {
-        this.claw.setPivotPosition(Claw.SCORE);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return true;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateReady.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateReady.java
deleted file mode 100644
index 093b285..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateReady.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package org.firstinspires.ftc.teamcode.commands.claw;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.command.CommandBase;
-
-import org.firstinspires.ftc.teamcode.subsystems.Claw;
-
-public class ClawRotateReady extends CommandBase {
-    private final MultipleTelemetry telemetry;
-    private final Claw claw;
-
-    public ClawRotateReady(final MultipleTelemetry telemetry, final Claw claw) {
-        this.telemetry = telemetry;
-
-        this.claw = claw;
-        super.addRequirements(claw);
-    }
-
-    @Override
-    public void initialize() {
-        this.claw.setPivotPosition(Claw.READY);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return true;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoAccepting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoAccepting.java
index 6663bc9..b99d573 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoAccepting.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoAccepting.java
@@ -1,17 +1,13 @@
 package org.firstinspires.ftc.teamcode.commands.extendo;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Extendo;
 
 public class ExtendoAccepting extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Extendo extendo;
 
-    public ExtendoAccepting(final MultipleTelemetry telemetry, final Extendo extendo) {
-        this.telemetry = telemetry;
-
+    public ExtendoAccepting(final Extendo extendo) {
         this.extendo = extendo;
         super.addRequirements(extendo);
     }
@@ -23,6 +19,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return this.extendo.isFinished();
+        return true;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoScore.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoBasket.java
similarity index 57%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoScore.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoBasket.java
index 77848e1..1dc2153 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoScore.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoBasket.java
@@ -1,17 +1,13 @@
 package org.firstinspires.ftc.teamcode.commands.extendo;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Extendo;
 
-public class ExtendoScore extends CommandBase {
-    private final MultipleTelemetry telemetry;
+public class ExtendoBasket extends CommandBase {
     private final Extendo extendo;
 
-    public ExtendoScore(final MultipleTelemetry telemetry, final Extendo extendo) {
-        this.telemetry = telemetry;
-
+    public ExtendoBasket(final Extendo extendo) {
         this.extendo = extendo;
         super.addRequirements(extendo);
     }
@@ -23,6 +19,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return this.extendo.isFinished();
+        return true;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoDriveIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoDriveIn.java
new file mode 100644
index 0000000..b116bd1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoDriveIn.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.extendo;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Extendo;
+
+public class ExtendoDriveIn extends CommandBase {
+    private final Extendo extendo;
+
+    public ExtendoDriveIn(final Extendo extendo) {
+        this.extendo = extendo;
+        super.addRequirements(extendo);
+    }
+
+    @Override
+    public void initialize() {
+        this.extendo.setPosition(Extendo.DRIVE_IN);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoReady.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoReady.java
index a7d47b6..1b401de 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoReady.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoReady.java
@@ -1,17 +1,13 @@
 package org.firstinspires.ftc.teamcode.commands.extendo;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Extendo;
 
 public class ExtendoReady extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Extendo extendo;
 
-    public ExtendoReady(final MultipleTelemetry telemetry, final Extendo extendo) {
-        this.telemetry = telemetry;
-
+    public ExtendoReady(final Extendo extendo) {
         this.extendo = extendo;
         super.addRequirements(extendo);
     }
@@ -23,6 +19,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return this.extendo.isFinished();
+        return true;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoSpecimen.java
new file mode 100644
index 0000000..dbfac73
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoSpecimen.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.extendo;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Extendo;
+
+public class ExtendoSpecimen extends CommandBase {
+    private final Extendo extendo;
+
+    public ExtendoSpecimen(final Extendo extendo) {
+        this.extendo = extendo;
+        super.addRequirements(extendo);
+    }
+
+    @Override
+    public void initialize() {
+        this.extendo.setPosition(Extendo.SPECIMEN);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoSweep.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoSweep.java
new file mode 100644
index 0000000..ae0b9e9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoSweep.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.extendo;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Extendo;
+
+public class ExtendoSweep extends CommandBase {
+    private final Extendo extendo;
+
+    public ExtendoSweep(final Extendo extendo) {
+        this.extendo = extendo;
+        super.addRequirements(extendo);
+    }
+
+    @Override
+    public void initialize() {
+        this.extendo.setPosition(Extendo.SWEEP);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftAccepting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftAccepting.java
index 355fa16..ecb530e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftAccepting.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftAccepting.java
@@ -1,18 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
 public class LiftAccepting extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftAccepting(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftAccepting(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDecrement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDecrement.java
index c833bb4..748e1ef 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDecrement.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDecrement.java
@@ -1,18 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
 public class LiftDecrement extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftDecrement(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftDecrement(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftReset.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDriveIn.java
similarity index 58%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftReset.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDriveIn.java
index 0b66f16..31a990b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftReset.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDriveIn.java
@@ -1,25 +1,21 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
-public class LiftReset extends CommandBase {
-    private final MultipleTelemetry telemetry;
+public class LiftDriveIn extends CommandBase {
     private final Lift lift;
 
-    public LiftReset(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftDriveIn(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
 
     @Override
     public void initialize() {
-        this.lift.reset();
+        this.lift.setTarget(Lift.DRIVE_IN);
     }
 
     @Override
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftHighBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftHighBasket.java
deleted file mode 100644
index 675d9c3..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftHighBasket.java
+++ /dev/null
@@ -1,29 +0,0 @@
-package org.firstinspires.ftc.teamcode.commands.lift;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.command.CommandBase;
-
-import org.firstinspires.ftc.teamcode.subsystems.Lift;
-
-
-public class LiftHighBasket extends CommandBase {
-    private final MultipleTelemetry telemetry;
-    private final Lift lift;
-
-    public LiftHighBasket(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
-        this.lift = lift;
-        super.addRequirements(lift);
-    }
-
-    @Override
-    public void initialize() {
-        this.lift.setTarget(Lift.HIGH_BASKET);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return this.lift.isFinished();
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftHighRung.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftHighRung.java
index 2c0c12e..26a5fe8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftHighRung.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftHighRung.java
@@ -1,18 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
 public class LiftHighRung extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftHighRung(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftHighRung(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftIncrement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftIncrement.java
index 23917f0..62149f0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftIncrement.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftIncrement.java
@@ -1,18 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
 public class LiftIncrement extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftIncrement(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftIncrement(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowBasket.java
index 96afa95..7a50663 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowBasket.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowBasket.java
@@ -1,18 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
 public class LiftLowBasket extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftLowBasket(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftLowBasket(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowRung.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowRung.java
index 2dfc95c..a0309ad 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowRung.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftLowRung.java
@@ -1,18 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
 public class LiftLowRung extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftLowRung(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftLowRung(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftSlam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftSlam.java
index 8623fb2..fa987e5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftSlam.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftSlam.java
@@ -1,17 +1,14 @@
 package org.firstinspires.ftc.teamcode.commands.lift;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandBase;
 
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
+
 public class LiftSlam extends CommandBase {
-    private final MultipleTelemetry telemetry;
     private final Lift lift;
 
-    public LiftSlam(final MultipleTelemetry telemetry, final Lift lift) {
-        this.telemetry = telemetry;
-
+    public LiftSlam(final Lift lift) {
         this.lift = lift;
         super.addRequirements(lift);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotAccepting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotAccepting.java
new file mode 100644
index 0000000..988eeea
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotAccepting.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.pivot;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+
+public class PivotAccepting extends CommandBase {
+    private final Pivot pivot;
+
+    public PivotAccepting(final Pivot pivot) {
+        this.pivot = pivot;
+        super.addRequirements(pivot);
+    }
+
+    @Override
+    public void initialize() {
+        this.pivot.setPosition(Pivot.ACCEPTING);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotBasket.java
new file mode 100644
index 0000000..fc020a4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotBasket.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.pivot;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+
+public class PivotBasket extends CommandBase {
+    private final Pivot pivot;
+
+    public PivotBasket(final Pivot pivot) {
+        this.pivot = pivot;
+        super.addRequirements(pivot);
+    }
+
+    @Override
+    public void initialize() {
+        this.pivot.setPosition(Pivot.SCORE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotDriveIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotDriveIn.java
new file mode 100644
index 0000000..af86299
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotDriveIn.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.pivot;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+
+public class PivotDriveIn extends CommandBase {
+    private final Pivot pivot;
+
+    public PivotDriveIn(final Pivot pivot) {
+        this.pivot = pivot;
+        super.addRequirements(pivot);
+    }
+
+    @Override
+    public void initialize() {
+        this.pivot.setPosition(Pivot.DRIVE_IN);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotIntake.java
new file mode 100644
index 0000000..59bb0a4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotIntake.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.pivot;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+
+public class PivotIntake extends CommandBase {
+    private final Pivot pivot;
+
+    public PivotIntake(final Pivot pivot) {
+        this.pivot = pivot;
+        super.addRequirements(pivot);
+    }
+
+    @Override
+    public void initialize() {
+        this.pivot.setPosition(Pivot.INTAKE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotReady.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotReady.java
new file mode 100644
index 0000000..a0d76e7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotReady.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.pivot;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+
+public class PivotReady extends CommandBase {
+    private final Pivot pivot;
+
+    public PivotReady(final Pivot pivot) {
+        this.pivot = pivot;
+        super.addRequirements(pivot);
+    }
+
+    @Override
+    public void initialize() {
+        this.pivot.setPosition(Pivot.READY);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotSpecimen.java
new file mode 100644
index 0000000..9e1f7ca
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotSpecimen.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.pivot;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+
+public class PivotSpecimen extends CommandBase {
+    private final Pivot pivot;
+
+    public PivotSpecimen(final Pivot pivot) {
+        this.pivot = pivot;
+        super.addRequirements(pivot);
+    }
+
+    @Override
+    public void initialize() {
+        this.pivot.setPosition(Pivot.SPECIMEN);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotSweep.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotSweep.java
new file mode 100644
index 0000000..8d57bca
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/pivot/PivotSweep.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.commands.pivot;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+
+public class PivotSweep extends CommandBase {
+    private final Pivot pivot;
+
+    public PivotSweep(final Pivot pivot) {
+        this.pivot = pivot;
+        super.addRequirements(pivot);
+    }
+
+    @Override
+    public void initialize() {
+        this.pivot.setPosition(Pivot.SWEEP);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristAccepting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristAccepting.java
new file mode 100644
index 0000000..da7961c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristAccepting.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.wrist;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
+
+public class WristAccepting extends CommandBase {
+    private final Wrist wrist;
+
+    public WristAccepting(final Wrist wrist) {
+        this.wrist = wrist;
+        super.addRequirements(wrist);
+    }
+
+    @Override
+    public void initialize() {
+        this.wrist.setPosition(Wrist.ACCEPTING);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristBasket.java
new file mode 100644
index 0000000..e3f9518
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristBasket.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.wrist;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
+
+public class WristBasket extends CommandBase {
+    private final Wrist wrist;
+
+    public WristBasket(final Wrist wrist) {
+        this.wrist = wrist;
+        super.addRequirements(wrist);
+    }
+
+    @Override
+    public void initialize() {
+        this.wrist.setPosition(Wrist.SCORE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristLeft.java
new file mode 100644
index 0000000..45a5340
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristLeft.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.commands.wrist;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
+
+public class WristLeft extends CommandBase {
+    private final Wrist wrist;
+
+    public WristLeft(final Wrist wrist) {
+        this.wrist = wrist;
+        super.addRequirements(wrist);
+    }
+
+    @Override
+    public void initialize() {
+        double target = this.wrist.getPosition() - Wrist.INCREMENT;
+        if (target < 0) target += 1;
+
+        this.wrist.setPosition(target);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristReady.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristReady.java
new file mode 100644
index 0000000..0e69696
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristReady.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.wrist;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
+
+public class WristReady extends CommandBase {
+    private final Wrist wrist;
+
+    public WristReady(final Wrist wrist) {
+        this.wrist = wrist;
+        super.addRequirements(wrist);
+    }
+
+    @Override
+    public void initialize() {
+        this.wrist.setPosition(Wrist.READY);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristRight.java
new file mode 100644
index 0000000..4083fcf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristRight.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.commands.wrist;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
+
+public class WristRight extends CommandBase {
+    private final Wrist wrist;
+
+    public WristRight(final Wrist wrist) {
+        this.wrist = wrist;
+        super.addRequirements(wrist);
+    }
+
+    @Override
+    public void initialize() {
+        double target = this.wrist.getPosition() + Wrist.INCREMENT;
+        if (target > 1) target -= 1;
+
+        this.wrist.setPosition(target);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristSpecimen.java
new file mode 100644
index 0000000..69709ed
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/wrist/WristSpecimen.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands.wrist;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
+
+public class WristSpecimen extends CommandBase {
+    private final Wrist wrist;
+
+    public WristSpecimen(final Wrist wrist) {
+        this.wrist = wrist;
+        super.addRequirements(wrist);
+    }
+
+    @Override
+    public void initialize() {
+        this.wrist.setPosition(Wrist.SPECIMEN);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java
deleted file mode 100644
index b1ec579..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java
+++ /dev/null
@@ -1,49 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.auton;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
-import org.firstinspires.ftc.teamcode.subsystems.Claw;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-
-@Config
-@Autonomous(name = "Park", preselectTeleOp = "Main")
-public class Park extends OpModeCore {
-    private MecanumDrive drive;
-    private ElapsedTime timer;
-    private Claw claw;
-
-    public static int DURATION = 3000;
-
-    @Override
-    public void initialize() {
-        this.drive = new MecanumDrive(super.hardwareMap, new Pose2d(0, 0, 0));
-        this.timer = new ElapsedTime();
-
-        this.claw = new Claw(super.hardwareMap, super.multipleTelemetry);
-    }
-
-    @Override
-    public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-        this.initialize();
-        super.waitForStart();
-
-        this.timer.reset();
-        while (super.opModeIsActive()) {
-            CommandScheduler.getInstance().run();
-
-            if (this.timer.milliseconds() <= Park.DURATION) {
-                this.drive.setPowers(-0.25, -0.25, -0.25, -0.25);
-            } else {
-                this.drive.setPowers(0, 0, 0, 0);
-            }
-        }
-
-        super.end();
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Push.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Push.java
new file mode 100644
index 0000000..433cdde
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Push.java
@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.opmodes.auton;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.CommandRobot;
+import org.firstinspires.ftc.teamcode.opmodes.auton.trajectories.SpecFiveTrajectories;
+import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
+import org.firstinspires.ftc.teamcode.utils.commands.PathChainCommand;
+import org.firstinspires.ftc.teamcode.utils.commands.PathCommand;
+
+@Config
+@Autonomous(name = "Push", preselectTeleOp = "Main")
+public class Push extends OpModeCore {
+    private CommandRobot robot;
+    private SpecFiveTrajectories trajectories;
+
+    public static double SPEED = 0.85;
+
+    @Override
+    public void initialize() {
+        this.trajectories = new SpecFiveTrajectories();
+
+        this.robot = new CommandRobot(super.hardwareMap, this.trajectories.getStart());
+    }
+
+    @Override
+    public void runOpMode() {
+        CommandScheduler.getInstance().enable();
+        this.initialize();
+
+        super.waitForStart();
+
+        this.robot.startAutoThreads(this);
+        CommandScheduler.getInstance().schedule(
+                new SequentialCommandGroup(
+                        new PathCommand(this.robot, this.trajectories.scorePreload, SPEED),
+                        new PathChainCommand(this.robot, SPEED, this.trajectories.setupTop, this.trajectories.pushTop),
+                        new PathChainCommand(this.robot, SPEED, this.trajectories.setupMid, this.trajectories.pushMid),
+                        new PathChainCommand(this.robot, SPEED, this.trajectories.setupBottom, this.trajectories.pushBottom),
+                        new PathCommand(this.robot, this.trajectories.intakeSecond, SPEED),
+                        new PathCommand(this.robot, this.trajectories.scoreSecond, SPEED),
+                        new PathCommand(this.robot, this.trajectories.intakeThird, SPEED),
+                        new PathCommand(this.robot, this.trajectories.scoreThird, SPEED),
+                        new PathCommand(this.robot, this.trajectories.intakeFourth, SPEED),
+                        new PathCommand(this.robot, this.trajectories.scoreFourth, SPEED),
+                        new PathCommand(this.robot, this.trajectories.park, SPEED)
+                )
+        );
+
+        while (opModeIsActive()) {
+            CommandScheduler.getInstance().run();
+        }
+
+        super.end();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecFive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecFive.java
new file mode 100644
index 0000000..09dc16a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecFive.java
@@ -0,0 +1,65 @@
+package org.firstinspires.ftc.teamcode.opmodes.auton;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.CommandRobot;
+import org.firstinspires.ftc.teamcode.opmodes.auton.trajectories.SpecFiveSweepTrajectories;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
+import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
+import org.firstinspires.ftc.teamcode.utils.commands.PathCommand;
+
+@Config
+@Autonomous(name = "5-Spec Far Basket", preselectTeleOp = "Main")
+public class SpecFive extends OpModeCore {
+    private CommandRobot robot;
+    private SpecFiveSweepTrajectories trajectories;
+
+    public static double SPEED = 0.6;
+
+    @Override
+    public void initialize() {
+        super.initialize();
+        this.trajectories = new SpecFiveSweepTrajectories();
+
+        robot = new CommandRobot(super.hardwareMap, this.trajectories.getStart());
+    }
+
+    @Override
+    public void runOpMode() {
+        CommandScheduler.getInstance().enable();
+        this.initialize();
+
+        super.waitForStart();
+
+        CommandScheduler.getInstance().schedule(
+                new SequentialCommandGroup(
+                        new PathCommand(this.robot, this.trajectories.scorePreload, SPEED),
+                        new PathCommand(this.robot, this.trajectories.setupTop, SPEED),
+                        new PathCommand(this.robot, this.trajectories.sweepTop, SPEED),
+                        new PathCommand(this.robot, this.trajectories.setupMid, SPEED),
+                        new PathCommand(this.robot, this.trajectories.sweepMid, SPEED),
+                        new PathCommand(this.robot, this.trajectories.setupBottom, SPEED),
+                        new PathCommand(this.robot, this.trajectories.sweepBottom, SPEED),
+                        new PathCommand(this.robot, this.trajectories.intakeSecond, SPEED),
+                        new PathCommand(this.robot, this.trajectories.scoreSecond, SPEED),
+                        new PathCommand(this.robot, this.trajectories.intakeThird, SPEED),
+                        new PathCommand(this.robot, this.trajectories.scoreThird, SPEED),
+                        new PathCommand(this.robot, this.trajectories.intakeFourth, SPEED),
+                        new PathCommand(this.robot, this.trajectories.scoreFourth, SPEED),
+                        new PathCommand(this.robot, this.trajectories.park, SPEED)
+                )
+        );
+
+        this.robot.startAutoThreads(this);
+        while (opModeIsActive()) {
+            TelemetryCore.getInstance().addLine(this.trajectories.poses.toString());
+            TelemetryCore.getInstance().update();
+            CommandScheduler.getInstance().run();
+        }
+
+        super.end();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecFour.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecFour.java
new file mode 100644
index 0000000..e999ef4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecFour.java
@@ -0,0 +1,147 @@
+package org.firstinspires.ftc.teamcode.opmodes.auton;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.command.ParallelCommandGroup;
+import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+import com.arcrobotics.ftclib.command.WaitCommand;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.CommandRobot;
+import org.firstinspires.ftc.teamcode.opmodes.auton.trajectories.SpecFourTrajectories;
+import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
+import org.firstinspires.ftc.teamcode.utils.commands.PathChainCommand;
+import org.firstinspires.ftc.teamcode.utils.commands.PathCommand;
+
+@Config
+@Autonomous(name = "4-Spec Far Basket", preselectTeleOp = "Main")
+public class SpecFour extends OpModeCore {
+    public static int HIGH_RUNG_WAIT = 0;
+    public static int SLAM_WAIT = 200;
+    public static int SPECIMEN_CLOSE_WAIT = 250;
+    public static int CYCLE_SPECIMEN_WAIT = 300;
+    public static int READY_WAIT = 500;
+
+    public static double SCORE_SPEED = 0.75;
+    public static double NORMAL_SPEED = 0.85;
+
+    private CommandRobot robot;
+    private SpecFourTrajectories trajectories;
+
+    @Override
+    public void initialize() {
+        this.trajectories = new SpecFourTrajectories();
+
+        robot = new CommandRobot(super.hardwareMap, this.trajectories.getStart());
+    }
+
+    @Override
+    public void runOpMode() {
+        CommandScheduler.getInstance().enable();
+        this.initialize();
+
+        super.waitForStart();
+
+        CommandScheduler.getInstance().schedule(
+                new SequentialCommandGroup(
+                        // PRELOAD
+                        this.robot.highRung(),
+                        new WaitCommand(HIGH_RUNG_WAIT),
+                        new PathCommand(this.robot, this.trajectories.scorePreload, SCORE_SPEED),
+                        this.robot.slam(),
+                        new WaitCommand(SLAM_WAIT),
+
+                        // PUSH SAMPLES
+                        new ParallelCommandGroup(
+                                new PathChainCommand(
+                                        this.robot,
+                                        NORMAL_SPEED,
+                                        this.trajectories.backFirst,
+                                        this.trajectories.setupTop,
+                                        this.trajectories.pushTop
+                                ),
+                                new SequentialCommandGroup(
+                                        new WaitCommand(READY_WAIT),
+                                        this.robot.ready()
+                                )
+                        ),
+
+                        new PathChainCommand(
+                                this.robot,
+                                NORMAL_SPEED,
+                                this.trajectories.setupMid,
+                                this.trajectories.pushMid
+                        ),
+
+
+                        // 2ND SPECIMEN
+                        new ParallelCommandGroup(
+                                new PathCommand(this.robot, this.trajectories.intakeSecond, NORMAL_SPEED),
+                                new SequentialCommandGroup(
+                                        new WaitCommand(CYCLE_SPECIMEN_WAIT),
+                                        this.robot.specimen()
+                                )
+                        ),
+                        new WaitCommand(SPECIMEN_CLOSE_WAIT),
+                        this.robot.close(),
+                        new WaitCommand(SPECIMEN_CLOSE_WAIT),
+                        this.robot.highRung(),
+                        new WaitCommand(HIGH_RUNG_WAIT),
+                        new PathCommand(this.robot, this.trajectories.scoreSecond, SCORE_SPEED),
+                        this.robot.slam(),
+                        new WaitCommand(SLAM_WAIT),
+
+                        // 3RD SPECIMEN
+                        new ParallelCommandGroup(
+                                new PathCommand(this.robot, this.trajectories.intakeThird, NORMAL_SPEED),
+                                new SequentialCommandGroup(
+                                        new WaitCommand(CYCLE_SPECIMEN_WAIT),
+                                        this.robot.specimen()
+                                )
+                        ),
+                        new WaitCommand(SPECIMEN_CLOSE_WAIT),
+                        this.robot.close(),
+                        new WaitCommand(SPECIMEN_CLOSE_WAIT),
+                        this.robot.highRung(),
+                        new WaitCommand(HIGH_RUNG_WAIT),
+                        new PathCommand(this.robot, this.trajectories.scoreThird, SCORE_SPEED),
+                        this.robot.slam(),
+                        new WaitCommand(SLAM_WAIT),
+
+
+                        // 4TH SPECIMEN
+                        new ParallelCommandGroup(
+                                new PathCommand(this.robot, this.trajectories.intakeFourth, NORMAL_SPEED),
+                                new SequentialCommandGroup(
+                                        new WaitCommand(CYCLE_SPECIMEN_WAIT),
+                                        this.robot.specimen()
+                                )
+                        ),
+                        new WaitCommand(SPECIMEN_CLOSE_WAIT),
+                        this.robot.close(),
+                        new WaitCommand(SPECIMEN_CLOSE_WAIT),
+                        this.robot.highRung(),
+                        new WaitCommand(HIGH_RUNG_WAIT),
+                        new PathCommand(this.robot, this.trajectories.scoreFourth, SCORE_SPEED),
+                        this.robot.slam(),
+                        new WaitCommand(SLAM_WAIT),
+
+                        // PARK
+                        new ParallelCommandGroup(
+                                new PathCommand(this.robot, this.trajectories.backFirst, NORMAL_SPEED),
+                                new SequentialCommandGroup(
+                                        new WaitCommand(READY_WAIT),
+                                        this.robot.ready()
+                                )
+                        )
+                )
+        );
+
+        this.robot.startAutoThreads(this);
+        while (opModeIsActive()) {
+            CommandScheduler.getInstance().run();
+        }
+
+        super.end();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java
deleted file mode 100644
index 53db62d..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/CloseBasket.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.auton.blue;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.firstinspires.ftc.teamcode.CommandRobot;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-
-@Autonomous(name = "Blue Close Basket", preselectTeleOp = "Main")
-public class CloseBasket extends OpModeCore {
-    private CommandRobot robot;
-
-    @Override
-    public void initialize() {
-        // TODO: Input correct starting position
-        this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this);
-    }
-
-    @Override
-    public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-        this.initialize();
-
-        super.waitForStart();
-
-        // TODO: Add auto pathing and functionality
-        // TODO: Make an end command for roadrunner (see kookybotz)?
-        // TODO: Research how to stop Action.runBlocking() without error
-
-        super.end();
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java
deleted file mode 100644
index 979975f..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/blue/FarBasket.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.auton.blue;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.firstinspires.ftc.teamcode.CommandRobot;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-
-@Autonomous(name = "Blue Far Bakset", preselectTeleOp = "Main")
-public class FarBasket extends OpModeCore {
-    private CommandRobot robot;
-
-    @Override
-    public void initialize() {
-        // TODO: Input correct starting position
-        this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this);
-    }
-
-    @Override
-    public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-        this.initialize();
-
-        super.waitForStart();
-
-        // TODO: Add auto pathing and functionality
-        // TODO: Make an end command for roadrunner (see kookybotz)?
-        // TODO: Research how to stop Action.runBlocking() without error
-
-        super.end();
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt
new file mode 100644
index 0000000..28c35b1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/notes.txt
@@ -0,0 +1,161 @@
+-------------------------------------------------------
+----NOTES FOR REFERENCE TO CALCULATE AUTON POSITION----
+-------------------------------------------------------
+
+ROBOT_WIDTH = 15.3
+ROBOT_LENGTH = 16
+CENTER_TO_CAMERA = 213.052 * 0.0394
+TILE_LENGTH = 23.0 // excluding "tabs"
+FIELD_LENGTH = 72.0
+
+                       forward (x positive)
+                                △
+                                |
+                                |
+                         /--------------\
+                         |              |
+                         |              |
+                         |           || |
+  left (y positive) <--- |           || |
+                         |     ____     |
+                         |     ----     |
+                         \--------------/
+
+
+@Autonomous(name = "0+5", preselectTeleOp = "Solo")
+public class ZeroPlusFive extends OpModeCore {
+    public static Path[] paths = new Path[16];
+
+    public void buildPaths() {
+        paths[0] = buildLine(
+                Constants.specimenStartPose,
+                new Pose(41.063, 65.625, 180),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[1] = buildCurve(
+                new Pose(41.063, 65.625, 180),
+                new Point(28.125, 50.063),
+                new Pose(46.500, 36.750, 270),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        paths[2] = buildLine(
+                new Pose(46.500, 36.750, 270),
+                new Pose(46.500, 36.750, 200),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        paths[3] = buildLine(
+                new Pose(46.500, 36.750, 200),
+                new Pose(46.500, 36.750, 270),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        paths[4] = buildLine(
+                new Pose(46.500, 36.750, 270),
+                new Pose(46.500, 36.750, 200),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        paths[5] = buildLine(
+                new Pose(46.500, 36.750, 200),
+                new Pose(46.500, 28.000, 270),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        paths[6] = buildLine(
+                new Pose(46.500, 28.000, 270),
+                new Pose(46.500, 28.000, 200),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        paths[7] = buildCurve(
+                new Pose(46.500, 28.000, 200),
+                new Point(26.625, 34.688),
+                new Pose(7.125, 36.375, 0),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        paths[8] = buildCurve(
+                new Pose(7.125, 36.375, 0),
+                new Point(28.313, 64.125),
+                new Pose(41.063, 64.500, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[9] = buildCurve(
+                new Pose(41.063, 64.500, 0),
+                new Point(28.313, 64.125),
+                new Pose(7.125, 36.375, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[10] = buildCurve(
+                new Pose(7.125, 36.375, 0),
+                new Point(28.313, 64.125),
+                new Pose(41.063, 67.500, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[11] = buildCurve(
+                new Pose(41.063, 67.500, 0),
+                new Point(28.313, 64.125),
+                new Pose(7.125, 36.375, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[12] = buildCurve(
+                new Pose(7.125, 36.375, 0),
+                new Point(28.313, 64.125),
+                new Pose(41.063, 69.500, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[13] = buildCurve(
+                new Pose(41.063, 69.500, 0),
+                new Point(28.313, 64.125),
+                new Pose(7.125, 36.375, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[14] = buildCurve(
+                new Pose(7.125, 36.375, 0),+
+                new Point(28.313, 64.125),
+                new Pose(41.063, 71.500, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        paths[15] = buildCurve(
+                new Pose(41.063, 71.500, 0),
+                new Point(13.875, 47.625),
+                new Pose(6.750, 37.500, 0),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+    }
+
+    public void initialize() {
+    }
+
+    @Override
+    public void runOpMode() {
+        Drivetrain drivetrain = new Drivetrain(hardwareMap, super.multipleTelemetry, Constants.specimenStartPose);
+        CommandScheduler.getInstance().reset();
+
+        buildPaths();
+
+        while (!isStarted()) {
+            CommandScheduler.getInstance().run();
+        }
+
+        CommandScheduler.getInstance().schedule(
+                new SequentialCommandGroup(
+                        new PathCommand(drivetrain.getFollower(), paths[0]),
+                        new PathCommand(drivetrain.getFollower(), paths[1]),
+                        new PathCommand(drivetrain.getFollower(), paths[2]),
+                        new PathCommand(drivetrain.getFollower(), paths[3]),
+                        new PathCommand(drivetrain.getFollower(), paths[4]),
+                        new PathCommand(drivetrain.getFollower(), paths[5]),
+                        new PathCommand(drivetrain.getFollower(), paths[6]),
+                        new PathCommand(drivetrain.getFollower(), paths[7]),
+                        new PathCommand(drivetrain.getFollower(), paths[8]),
+                        new PathCommand(drivetrain.getFollower(), paths[9]),
+                        new PathCommand(drivetrain.getFollower(), paths[10]),
+                        new PathCommand(drivetrain.getFollower(), paths[11]),
+                        new PathCommand(drivetrain.getFollower(), paths[12]),
+                        new PathCommand(drivetrain.getFollower(), paths[13]),
+                        new PathCommand(drivetrain.getFollower(), paths[14]),
+                        new PathCommand(drivetrain.getFollower(), paths[15])
+                )
+        );
+
+        while (opModeIsActive() && !isStopRequested()) {
+            CommandScheduler.getInstance().run();
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java
deleted file mode 100644
index b21976b..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/CloseBasket.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.auton.red;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.firstinspires.ftc.teamcode.CommandRobot;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-
-@Autonomous(name = "Red Close Basket", preselectTeleOp = "Main")
-public class CloseBasket extends OpModeCore {
-    private CommandRobot robot;
-
-    @Override
-    public void initialize() {
-        // TODO: Input correct starting position
-        this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this);
-    }
-
-    @Override
-    public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-        this.initialize();
-
-        super.waitForStart();
-
-        // TODO: Add auto pathing and functionality
-        // TODO: Make an end command for roadrunner (see kookybotz)?
-        // TODO: Research how to stop Action.runBlocking() without error
-
-        super.end();
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java
deleted file mode 100644
index b04b1cc..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/red/FarBasket.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.auton.red;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.firstinspires.ftc.teamcode.CommandRobot;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-
-@Autonomous(name = "Red Far Basket", preselectTeleOp = "Main")
-public class FarBasket extends OpModeCore {
-    private CommandRobot robot;
-
-    @Override
-    public void initialize() {
-        // TODO: Input correct starting position
-        this.robot = new CommandRobot(super.hardwareMap, new Pose2d(0, 0, 0), super.multipleTelemetry, this);
-    }
-
-    @Override
-    public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-        this.initialize();
-
-        super.waitForStart();
-
-        // TODO: Add auto pathing and functionality
-        // TODO: Make an end command for roadrunner (see kookybotz)?
-        // TODO: Research how to stop Action.runBlocking() without error
-
-        super.end();
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFiveSweepTrajectories.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFiveSweepTrajectories.java
new file mode 100644
index 0000000..158c267
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFiveSweepTrajectories.java
@@ -0,0 +1,164 @@
+package org.firstinspires.ftc.teamcode.opmodes.auton.trajectories;
+
+import static org.firstinspires.ftc.teamcode.utils.AutonomousHelpers.buildCurve;
+import static org.firstinspires.ftc.teamcode.utils.AutonomousHelpers.buildLine;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.pathgen.Path;
+
+import org.firstinspires.ftc.teamcode.utils.AutonomousHelpers;
+
+import java.io.File;
+import java.util.ArrayList;
+
+@Config
+public class SpecFiveSweepTrajectories {
+    public final ArrayList<Pose> poses;
+
+    public static double SCORE_PRELOAD = 1.4;
+    public static double SETUP_TOP = 3;
+    public static double SCORE_SECOND = 1.5;
+    public static double SCORE_THIRD = 1.5;
+    public static double SCORE_FOURTH = 1.5;
+    public static double BACK_FIRST = 1.1;
+    public static double SWEEP_TOP = 3;
+    public static double SETUP_MID = 3;
+    public static double SWEEP_MID = 3;
+    public static double SETUP_BOTTOM = 3;
+    public static double SWEEP_BOTTOM = 3;
+
+    public Path scorePreload, intakeSecond, scoreSecond, intakeThird, scoreThird, intakeFourth, scoreFourth, intakeFifth, scoreFifth, park, setupTop, sweepTop, setupMid, sweepMid, setupBottom, sweepBottom;
+
+    public SpecFiveSweepTrajectories() {
+        this.poses = AutonomousHelpers.getPoses(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/spec5sweep.pp"));
+
+        this.scorePreload = buildCurve(
+                poses.get(0),
+                AutonomousHelpers.poseToPoint(poses.get(2)),
+                AutonomousHelpers.poseToPoint(poses.get(3)),
+                poses.get(1),
+                AutonomousHelpers.HeadingInterpolation.CONSTANT
+        );
+        this.scorePreload.setPathEndTimeoutConstraint(SCORE_PRELOAD);
+
+        this.setupTop = buildCurve(
+                poses.get(1),
+                AutonomousHelpers.poseToPoint(poses.get(5)),
+                poses.get(4),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.setupTop.setPathEndTimeoutConstraint(SETUP_TOP);
+
+        this.sweepTop = buildLine(
+                poses.get(4),
+                poses.get(6),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.sweepTop.setPathEndTimeoutConstraint(SWEEP_TOP);
+
+        this.setupMid = buildLine(
+                poses.get(6),
+                poses.get(7),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.setupMid.setPathEndTimeoutConstraint(SETUP_MID);
+
+        this.sweepMid = buildLine(
+                poses.get(7),
+                poses.get(8),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.setupMid.setPathEndTimeoutConstraint(SWEEP_MID);
+
+        this.setupBottom = buildLine(
+                poses.get(8),
+                poses.get(9),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.setupMid.setPathEndTimeoutConstraint(SETUP_BOTTOM);
+
+        this.sweepBottom = buildLine(
+                poses.get(9),
+                poses.get(10),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.setupMid.setPathEndTimeoutConstraint(SWEEP_BOTTOM);
+
+        this.intakeSecond = buildCurve(
+                poses.get(10),
+                AutonomousHelpers.poseToPoint(poses.get(12)),
+                poses.get(11),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreSecond = buildCurve(
+                poses.get(11),
+                AutonomousHelpers.poseToPoint(poses.get(14)),
+                poses.get(13),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.scoreSecond.setPathEndTimeoutConstraint(SCORE_SECOND);
+
+        this.intakeThird = buildCurve(
+                poses.get(13),
+                AutonomousHelpers.poseToPoint(poses.get(16)),
+                AutonomousHelpers.poseToPoint(poses.get(17)),
+                poses.get(15),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreThird = buildCurve(
+                poses.get(15),
+                AutonomousHelpers.poseToPoint(poses.get(19)),
+                poses.get(18),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.scoreThird.setPathEndTimeoutConstraint(SCORE_THIRD);
+
+        this.intakeFourth = buildCurve(
+                poses.get(18),
+                AutonomousHelpers.poseToPoint(poses.get(21)),
+                AutonomousHelpers.poseToPoint(poses.get(22)),
+                poses.get(20),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreFourth = buildCurve(
+                poses.get(20),
+                AutonomousHelpers.poseToPoint(poses.get(24)),
+                poses.get(23),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.scoreFourth.setPathEndTimeoutConstraint(SCORE_FOURTH);
+
+        this.intakeFifth = buildCurve(
+                poses.get(23),
+                AutonomousHelpers.poseToPoint(poses.get(26)),
+                AutonomousHelpers.poseToPoint(poses.get(27)),
+                poses.get(25),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+
+        this.scoreFifth = buildCurve(
+                poses.get(25),
+                AutonomousHelpers.poseToPoint(poses.get(29)),
+                poses.get(28),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.park = buildCurve(
+                poses.get(28),
+                AutonomousHelpers.poseToPoint(poses.get(31)),
+                AutonomousHelpers.poseToPoint(poses.get(32)),
+                poses.get(30),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+    }
+
+    public Pose getStart() {
+        return this.poses.get(0);
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFiveTrajectories.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFiveTrajectories.java
new file mode 100644
index 0000000..d6519f6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFiveTrajectories.java
@@ -0,0 +1,151 @@
+package org.firstinspires.ftc.teamcode.opmodes.auton.trajectories;
+
+import static org.firstinspires.ftc.teamcode.utils.AutonomousHelpers.buildCurve;
+import static org.firstinspires.ftc.teamcode.utils.AutonomousHelpers.buildLine;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.pathgen.Path;
+
+import org.firstinspires.ftc.teamcode.utils.AutonomousHelpers;
+
+import java.io.File;
+import java.util.ArrayList;
+
+@Config
+public class SpecFiveTrajectories {
+    private final ArrayList<Pose> poses;
+    public static double SCORE_PRELOAD = 1.4;
+    public static double SCORE_SECOND = 1.5;
+    public static double SCORE_THIRD = 1.5;
+    public static double SCORE_FOURTH = 1.5;
+    public static double BACK_FIRST = 1.1;
+
+    public Path scorePreload, intakeSecond, scoreSecond, intakeThird, scoreThird, intakeFourth, scoreFourth, intakeFifth, scoreFifth, park, setupTop, pushTop, setupMid, pushMid, setupBottom, pushBottom;
+
+    public SpecFiveTrajectories() {
+        this.poses = AutonomousHelpers.getPoses(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/spec5.pp"));
+
+        this.scorePreload = buildCurve(
+                poses.get(0),
+                AutonomousHelpers.poseToPoint(poses.get(2)),
+                AutonomousHelpers.poseToPoint(poses.get(3)),
+                poses.get(1),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.setupTop = buildCurve(
+                poses.get(1),
+                AutonomousHelpers.poseToPoint(poses.get(5)),
+                AutonomousHelpers.poseToPoint(poses.get(6)),
+                AutonomousHelpers.poseToPoint(poses.get(7)),
+                poses.get(4),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.pushTop = buildLine(
+                poses.get(4),
+                poses.get(8),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.setupMid = buildCurve(
+                poses.get(8),
+                AutonomousHelpers.poseToPoint(poses.get(10)),
+                poses.get(9),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.pushMid = buildLine(
+                poses.get(9),
+                poses.get(11),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.setupBottom = buildCurve(
+                poses.get(11),
+                AutonomousHelpers.poseToPoint(poses.get(13)),
+                poses.get(12),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.pushBottom = buildLine(
+                poses.get(12),
+                poses.get(14),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.intakeSecond = buildCurve(
+                poses.get(14),
+                AutonomousHelpers.poseToPoint(poses.get(16)),
+                poses.get(15),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreSecond = buildCurve(
+                poses.get(15),
+                AutonomousHelpers.poseToPoint(poses.get(18)),
+                poses.get(17),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.intakeThird = buildCurve(
+                poses.get(17),
+                AutonomousHelpers.poseToPoint(poses.get(20)),
+                AutonomousHelpers.poseToPoint(poses.get(21)),
+                poses.get(19),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreThird = buildCurve(
+                poses.get(19),
+                AutonomousHelpers.poseToPoint(poses.get(23)),
+                poses.get(22),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.intakeFourth = buildCurve(
+                poses.get(22),
+                AutonomousHelpers.poseToPoint(poses.get(25)),
+                AutonomousHelpers.poseToPoint(poses.get(26)),
+                poses.get(24),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreFourth = buildCurve(
+                poses.get(24),
+                AutonomousHelpers.poseToPoint(poses.get(28)),
+                poses.get(27),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.intakeFifth = buildCurve(
+                poses.get(27),
+                AutonomousHelpers.poseToPoint(poses.get(30)),
+                AutonomousHelpers.poseToPoint(poses.get(31)),
+                poses.get(29),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+
+        this.scoreFifth = buildCurve(
+                poses.get(29),
+                AutonomousHelpers.poseToPoint(poses.get(33)),
+                poses.get(32),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.park = buildCurve(
+                poses.get(32),
+                AutonomousHelpers.poseToPoint(poses.get(35)),
+                AutonomousHelpers.poseToPoint(poses.get(36)),
+                poses.get(34),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+    }
+
+    public Pose getStart() {
+        return this.poses.get(0);
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFourTrajectories.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFourTrajectories.java
new file mode 100644
index 0000000..876ae7e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/SpecFourTrajectories.java
@@ -0,0 +1,144 @@
+package org.firstinspires.ftc.teamcode.opmodes.auton.trajectories;
+
+import static org.firstinspires.ftc.teamcode.utils.AutonomousHelpers.buildCurve;
+import static org.firstinspires.ftc.teamcode.utils.AutonomousHelpers.buildLine;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.pathgen.Path;
+
+import org.firstinspires.ftc.teamcode.utils.AutonomousHelpers;
+
+import java.io.File;
+import java.util.ArrayList;
+
+@Config
+public class SpecFourTrajectories {
+    private final ArrayList<Pose> poses;
+
+    public static double SCORE_PRELOAD = 1.4;
+    public static double SCORE_SECOND = 1.5;
+    public static double SCORE_THIRD = 1.5;
+    public static double SCORE_FOURTH = 1.5;
+    public static double BACK_FIRST = 1.1;
+
+    public Path scorePreload, intakeSecond, scoreSecond, intakeThird, scoreThird, intakeFourth, scoreFourth, park, setupTop, pushTop, setupMid, pushMid, setupBottom, pushBottom, backFirst;
+
+    public SpecFourTrajectories() {
+        this.poses = AutonomousHelpers.getPoses(new File("").getAbsolutePath().concat("/sdcard/FIRST/positions/spec4.pp"));
+
+        this.scorePreload = buildCurve(
+                poses.get(0),
+                AutonomousHelpers.poseToPoint(poses.get(2)),
+                AutonomousHelpers.poseToPoint(poses.get(3)),
+                poses.get(1),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.scorePreload.setPathEndTimeoutConstraint(SCORE_PRELOAD);
+
+        this.backFirst = buildLine(
+                poses.get(1),
+                poses.get(4),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.backFirst.setPathEndTimeoutConstraint(BACK_FIRST);
+
+        this.setupTop = buildCurve(
+                poses.get(4),
+                AutonomousHelpers.poseToPoint(poses.get(6)),
+                AutonomousHelpers.poseToPoint(poses.get(7)),
+                poses.get(5),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.pushTop = buildLine(
+                poses.get(5),
+                poses.get(8),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.setupMid = buildCurve(
+                poses.get(8),
+                AutonomousHelpers.poseToPoint(poses.get(10)),
+                poses.get(9),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.pushMid = buildLine(
+                poses.get(9),
+                poses.get(11),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.setupBottom = buildCurve(
+                poses.get(11),
+                AutonomousHelpers.poseToPoint(poses.get(13)),
+                poses.get(12),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.pushBottom = buildLine(
+                poses.get(12),
+                poses.get(14),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.intakeSecond = buildCurve(
+                poses.get(11),
+                AutonomousHelpers.poseToPoint(poses.get(16)),
+                poses.get(15),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreSecond = buildCurve(
+                poses.get(15),
+                AutonomousHelpers.poseToPoint(poses.get(18)),
+                poses.get(17),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.scoreSecond.setPathEndTimeoutConstraint(SCORE_SECOND);
+
+        this.intakeThird = buildCurve(
+                poses.get(17),
+                AutonomousHelpers.poseToPoint(poses.get(20)),
+                AutonomousHelpers.poseToPoint(poses.get(21)),
+                poses.get(19),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreThird = buildCurve(
+                poses.get(19),
+                AutonomousHelpers.poseToPoint(poses.get(23)),
+                poses.get(22),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.scoreThird.setPathEndTimeoutConstraint(SCORE_THIRD);
+
+        this.intakeFourth = buildCurve(
+                poses.get(22),
+                AutonomousHelpers.poseToPoint(poses.get(25)),
+                AutonomousHelpers.poseToPoint(poses.get(26)),
+                poses.get(24),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+
+        this.scoreFourth = buildCurve(
+                poses.get(24),
+                AutonomousHelpers.poseToPoint(poses.get(28)),
+                poses.get(27),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+        this.scoreFourth.setPathEndTimeoutConstraint(SCORE_FOURTH);
+
+        this.park = buildCurve(
+                poses.get(27),
+                AutonomousHelpers.poseToPoint(poses.get(30)),
+                poses.get(29),
+                AutonomousHelpers.HeadingInterpolation.LINEAR
+        );
+    }
+
+    public Pose getStart() {
+        return this.poses.get(0);
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/close-basket.pp b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/close-basket.pp
new file mode 100644
index 0000000..5bc1525
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/close-basket.pp
@@ -0,0 +1,131 @@
+{
+    "startPoint": {
+        "x": 8,
+        "y": 100,
+        "heading": "linear",
+        "startDeg": 0,
+        "endDeg": 0,
+        "index": 0
+    },
+    "lines": [
+        {
+            "endPoint": {
+                "x": 17,
+                "y": 125,
+                "heading": "linear",
+                "startDeg": 0,
+                "endDeg": -45,
+                "name": "score preload",
+                "index": 1
+            },
+            "controlPoints": [],
+            "color": "#DBD967"
+        },
+        {
+            "endPoint": {
+                "x": 35,
+                "y": 121,
+                "heading": "linear",
+                "reverse": false,
+                "endDeg": 0,
+                "degrees": 0,
+                "startDeg": -45,
+                "name": "grab 1",
+                "index": 2
+            },
+            "controlPoints": [],
+            "color": "#75BA9C"
+        },
+        {
+            "endPoint": {
+                "x": 17,
+                "y": 125,
+                "heading": "linear",
+                "reverse": false,
+                "endDeg": -45,
+                "startDeg": 0,
+                "name": "score 1",
+                "index": 3
+            },
+            "controlPoints": [],
+            "color": "#BAAC88"
+        },
+        {
+            "endPoint": {
+                "x": 35,
+                "y": 132,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": -45,
+                "degrees": 0,
+                "endDeg": 0,
+                "name": "grab 2",
+                "index": 4
+            },
+            "controlPoints": [],
+            "color": "#9B9CB6"
+        },
+        {
+            "endPoint": {
+                "x": 17,
+                "y": 125,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": -45,
+                "name": "score 2",
+                "index": 5
+            },
+            "controlPoints": [],
+            "color": "#86A7D8"
+        },
+        {
+            "endPoint": {
+                "x": 46,
+                "y": 129,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": -45,
+                "endDeg": 90,
+                "name": "grab 3",
+                "index": 6
+            },
+            "controlPoints": [],
+            "color": "#8CDCC5"
+        },
+        {
+            "endPoint": {
+                "x": 17,
+                "y": 125,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 90,
+                "endDeg": -45,
+                "name": "score 3",
+                "index": 7
+            },
+            "controlPoints": [],
+            "color": "#C7CC79"
+        },
+        {
+            "endPoint": {
+                "x": 72,
+                "y": 95,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": -45,
+                "endDeg": -90,
+                "name": "park",
+                "index": 8
+            },
+            "controlPoints": [
+                {
+                    "x": 72,
+                    "y": 130,
+                    "index": 9
+                }
+            ],
+            "color": "#5667AB"
+        }
+    ]
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/push.pp b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/push.pp
new file mode 100644
index 0000000..e69de29
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec4.pp b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec4.pp
new file mode 100644
index 0000000..d9969b8
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec4.pp
@@ -0,0 +1,323 @@
+{
+    "startPoint": {
+        "index": 0,
+        "name": "start",
+        "x": 10,
+        "y": 64,
+        "heading": "constant",
+        "degrees": 0
+    },
+    "lines": [
+        {
+            "endPoint": {
+                "index": 1,
+                "name": "scoreFirst",
+                "x": 42,
+                "y": 72,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "x": 25,
+                    "y": 62,
+                    "name": "scoreFirstControl",
+                    "index": 2
+                },
+                {
+                    "x": 25,
+                    "y": 75,
+                    "name": "scoreFirstControlSecond",
+                    "index": 3
+                }
+            ],
+            "color": "#7BC766"
+        },
+        {
+            "endPoint": {
+                "index": 4,
+                "name": "backFirst",
+                "x": 25,
+                "y": 72,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [],
+            "color": "#7BC766"
+        },
+        {
+            "endPoint": {
+                "index": 5,
+                "name": "setupTop",
+                "x": 55,
+                "y": 28,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 6,
+                    "name": "setupTopControlFirst",
+                    "x": 20,
+                    "y": 10.5
+                },
+                {
+                    "index": 7,
+                    "name": "setupTopControlSecond",
+                    "x": 63,
+                    "y": 51.4
+                }
+            ],
+            "color": "#A77BAA"
+        },
+        {
+            "endPoint": {
+                "index": 8,
+                "name": "pushTop",
+                "x": 15,
+                "y": 28,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [],
+            "color": "#C86A59"
+        },
+        {
+            "endPoint": {
+                "index": 9,
+                "name": "setupMid",
+                "x": 55,
+                "y": 17,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 10,
+                    "name": "setupMidControl",
+                    "x": 65,
+                    "y": 34
+                }
+            ],
+            "color": "#8CD896"
+        },
+        {
+            "endPoint": {
+                "index": 11,
+                "name": "pushMid",
+                "x": 15,
+                "y": 17,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [],
+            "color": "#5B5C69"
+        },
+        {
+            "endPoint": {
+                "index": 12,
+                "name": "setupBottom",
+                "x": 55,
+                "y": 13.5,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 13,
+                    "name": "setupBottomControl",
+                    "x": 65,
+                    "y": 21
+                }
+            ],
+            "color": "#B89AC6"
+        },
+        {
+            "endPoint": {
+                "index": 14,
+                "name": "pushBottom",
+                "x": 15,
+                "y": 13.5,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [],
+            "color": "#9AB8C6"
+        },
+        {
+            "endPoint": {
+                "index": 15,
+                "name": "intakeSecond",
+                "x": 11,
+                "y": 34,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 16,
+                    "name": "intakeSecondControl",
+                    "x": 20,
+                    "y": 37
+                }
+            ],
+            "color": "#7C5CCC"
+        },
+        {
+            "endPoint": {
+                "index": 17,
+                "name": "scoreSecond",
+                "x": 40,
+                "y": 71,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 18,
+                    "name": "scoreSecondControl",
+                    "x": 20,
+                    "y": 67
+                }
+            ],
+            "color": "#D8B6C5"
+        },
+        {
+            "endPoint": {
+                "index": 19,
+                "name": "intakeThird",
+                "x": 11,
+                "y": 34,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 20,
+                    "name": "intakeThirdControlFirst",
+                    "x": 20,
+                    "y": 67
+                },
+                {
+                    "x": 30,
+                    "y": 30,
+                    "name": "intakeThirdControlSecond",
+                    "index": 21
+                }
+            ],
+            "color": "#C5557B"
+        },
+        {
+            "endPoint": {
+                "index": 22,
+                "name": "scoreThird",
+                "x": 40,
+                "y": 70,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 23,
+                    "name": "scoreThirdControl",
+                    "x": 20,
+                    "y": 65
+                }
+            ],
+            "color": "#657CA8"
+        },
+        {
+            "endPoint": {
+                "index": 24,
+                "name": "intakeFourth",
+                "x": 11,
+                "y": 34,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 25,
+                    "name": "intakeFourthControlFirst",
+                    "x": 20,
+                    "y": 65
+                },
+                {
+                    "x": 30,
+                    "y": 30,
+                    "name": "intakeFourthControlSecond",
+                    "index": 26
+                }
+            ],
+            "color": "#566C8A"
+        },
+        {
+            "endPoint": {
+                "index": 27,
+                "name": "scoreFourth",
+                "x": 40,
+                "y": 69,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 28,
+                    "name": "scoreFourthControl",
+                    "x": 20,
+                    "y": 63
+                }
+            ],
+            "color": "#6A9C7B"
+        },
+        {
+            "endPoint": {
+                "index": 29,
+                "name": "park",
+                "x": 11,
+                "y": 34,
+                "heading": "linear",
+                "reverse": false,
+                "startDeg": 0,
+                "endDeg": 0
+            },
+            "controlPoints": [
+                {
+                    "index": 30,
+                    "name": "parkControl",
+                    "x": 20,
+                    "y": 63
+                }
+            ],
+            "color": "#D758AA"
+        }
+    ]
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5.pp b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5.pp
new file mode 100644
index 0000000..1ae8bfc
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5.pp
@@ -0,0 +1,368 @@
+{
+  "startPoint": {
+  "x": 8,
+  "y": 65,
+  "heading": "linear",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 0
+},
+"lines": [
+  {
+    "endPoint": {
+    "x": 42,
+    "y": 72,
+    "heading": "linear",
+    "startDeg": 0,
+    "endDeg": 0,
+    "name": "scorePreload",
+    "index": 1
+  },
+  "controlPoints": [
+  {
+  "x": 25,
+  "y": 62,
+  "name": "scorePreloadFirstControl",
+  "index": 2
+  },
+  {
+  "x": 25,
+  "y": 75,
+  "name": "scorePreloadSecondControl",
+  "index": 3
+  }
+],
+"color": "#6B9AD7"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 24,
+  "heading": "linear",
+  "reverse": false,
+  "name": "setupTop",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 4
+},
+"controlPoints": [
+  {
+    "x": 35,
+    "y": 72,
+    "name": "setupTopFirstControl",
+    "index": 5
+  },
+  {
+    "x": 20,
+    "y": 7.8,
+    "name": "setupTopSecondControl",
+    "index": 6
+  },
+  {
+    "x": 56,
+    "y": 51,
+    "name": "setupTopThirdControl",
+    "index": 7
+  }
+],
+"color": "#C97AD7"
+},
+{
+  "endPoint": {
+  "x": 12,
+  "y": 24,
+  "heading": "linear",
+  "reverse": false,
+  "name": "pushTop",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 8
+},
+"controlPoints": [],
+"color": "#987A9B"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 14,
+  "heading": "linear",
+  "reverse": false,
+  "name": "setupMid",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 9
+},
+"controlPoints": [
+  {
+    "x": 35.44615384615385,
+    "y": 21.876923076923077,
+    "name": "setupMidControl",
+    "index": 10
+  }
+],
+"color": "#9AA56D"
+},
+{
+  "endPoint": {
+  "x": 12,
+  "y": 14,
+  "heading": "linear",
+  "reverse": false,
+  "name": "pushMid",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 11
+},
+"controlPoints": [],
+"color": "#6DC56B"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 8,
+  "heading": "linear",
+  "reverse": false,
+  "name": "setupBottom",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 12
+},
+"controlPoints": [
+  {
+    "x": 30.184615384615384,
+    "y": 15.5076923076923,
+    "name": "setupBottomControl",
+    "index": 13
+  }
+],
+"color": "#7BB679"
+},
+{
+  "endPoint": {
+  "x": 12,
+  "y": 8,
+  "heading": "linear",
+  "reverse": false,
+  "name": "pushBottom",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 14
+},
+"controlPoints": [],
+"color": "#85C966"
+},
+{
+  "endPoint": {
+  "x": 8,
+  "y": 34,
+  "heading": "linear",
+  "reverse": false,
+  "name": "intakeSecond",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 15
+},
+"controlPoints": [
+  {
+    "x": 9.415384615384616,
+    "y": 25.476923076923082,
+    "name": "intakeSecondControl",
+    "index": 16
+  }
+],
+"color": "#56765A"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 71,
+  "heading": "linear",
+  "reverse": false,
+  "startDeg": 0,
+  "endDeg": 0,
+  "name": "scoreSecond",
+  "index": 17
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 67,
+    "name": "scoreSecondControl",
+    "index": 18
+  }
+],
+"color": "#95886D"
+},
+{
+  "endPoint": {
+  "x": 8,
+  "y": 34,
+  "heading": "linear",
+  "reverse": false,
+  "name": "intakeThird",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 19
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "intakeThirdFirstControl",
+    "index": 20
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "intakeThirdSecondControl",
+    "index": 21
+  }
+],
+"color": "#DA67A9"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 70,
+  "heading": "linear",
+  "reverse": false,
+  "name": "scoreThird",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 22
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 65,
+    "name": "scoreThirdControl",
+    "index": 23
+  }
+],
+"color": "#A65596"
+},
+{
+  "endPoint": {
+  "x": 8,
+  "y": 34,
+  "heading": "linear",
+  "reverse": false,
+  "name": "intakeFourth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 24
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "intakeFourthFirstControl",
+    "index": 25
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "intakeFourthSecondControl",
+    "index": 26
+  }
+],
+"color": "#88C58B"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 69,
+  "heading": "linear",
+  "reverse": false,
+  "name": "scoreFourth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 27
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 63,
+    "name": "scoreFourthControl",
+    "index": 28
+  }
+],
+"color": "#D9B96B"
+},
+{
+  "endPoint": {
+  "x": 8,
+  "y": 34,
+  "heading": "linear",
+  "reverse": false,
+  "name": "intakeFifth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 29
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "intakeFifthFirstControl",
+    "index": 30
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "intakeFifthSecondControl",
+    "index": 31
+  }
+],
+"color": "#D5568A"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 68,
+  "heading": "linear",
+  "reverse": false,
+  "name": "scoreFifth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 32
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 61,
+    "name": "scoreFifthControl",
+    "index": 33
+  }
+],
+"color": "#B677D7"
+},
+{
+  "endPoint": {
+  "x": 8,
+  "y": 34,
+  "heading": "linear",
+  "reverse": false,
+  "name": "park",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 34
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "parkControl",
+    "index": 35
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "parkSecondControl",
+    "index": 36
+  }
+],
+"color": "#D8B67C"
+}
+]
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5sweep.pp b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5sweep.pp
new file mode 100644
index 0000000..92cb011
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/trajectories/positions/spec5sweep.pp
@@ -0,0 +1,349 @@
+{
+  "startPoint": {
+  "x": 8,
+  "y": 65,
+  "heading": "linear",
+  "startDeg": 0,
+  "endDeg": 0,
+  "index": 0
+},
+"lines": [
+  {
+    "endPoint": {
+    "x": 42,
+    "y": 72,
+    "heading": "constant",
+    "startDeg": 0,
+    "endDeg": 0,
+    "name": "scorePreload",
+    "degrees": 0,
+    "index": 1
+  },
+  "controlPoints": [
+  {
+  "x": 25,
+  "y": 62,
+  "name": "scorePreloadFirstControl",
+  "index": 2
+  },
+  {
+  "x": 25,
+  "y": 75,
+  "name": "scorePreloadSecondControl",
+  "index": 3
+  }
+],
+"color": "#DCDC7A"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 45,
+  "heading": "linear",
+  "reverse": false,
+  "name": "setupTop",
+  "startDeg": 0,
+  "endDeg": 295,
+  "index": 4
+},
+"controlPoints": [
+  {
+    "x": 25,
+    "y": 50,
+    "name": "setupTopControl",
+    "index": 5
+  }
+],
+"color": "#8A7965"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 45,
+  "heading": "linear",
+  "reverse": false,
+  "name": "sweepTop",
+  "startDeg": 295,
+  "endDeg": 227,
+  "index": 6
+},
+"controlPoints": [],
+"color": "#A6ABAA"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 35,
+  "heading": "linear",
+  "reverse": false,
+  "name": "setupMid",
+  "startDeg": 227,
+  "endDeg": 295,
+  "index": 7
+},
+"controlPoints": [],
+"color": "#DBBD97"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 35,
+  "heading": "linear",
+  "reverse": false,
+  "name": "sweepMid",
+  "startDeg": 295,
+  "endDeg": 227,
+  "index": 8
+},
+"controlPoints": [],
+"color": "#CB685C"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 25,
+  "heading": "linear",
+  "reverse": false,
+  "name": "setupBottom",
+  "startDeg": 227,
+  "endDeg": 295,
+  "index": 9
+},
+"controlPoints": [],
+"color": "#A5AAA5"
+},
+{
+  "endPoint": {
+  "x": 35,
+  "y": 25,
+  "heading": "linear",
+  "reverse": false,
+  "name": "sweepBottom",
+  "startDeg": 295,
+  "endDeg": 227,
+  "index": 10
+},
+"controlPoints": [],
+"color": "#6D7B67"
+},
+{
+  "endPoint": {
+  "x": 9,
+  "y": 34,
+  "heading": "linear",
+  "reverse": false,
+  "name": "intakeSecond",
+  "startDeg": 227,
+  "endDeg": 0,
+  "index": 11
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 37,
+    "name": "intakeSecondControl",
+    "index": 12
+  }
+],
+"color": "#5CA779"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 71,
+  "heading": "constant",
+  "reverse": false,
+  "name": "scoreSecond",
+  "startDeg": 0,
+  "endDeg": 0,
+  "degrees": 0,
+  "index": 13
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 67,
+    "name": "scoreSecondControl",
+    "index": 14
+  }
+],
+"color": "#8B769A"
+},
+{
+  "endPoint": {
+  "x": 9,
+  "y": 34,
+  "heading": "constant",
+  "reverse": false,
+  "name": "intakeThird",
+  "startDeg": 0,
+  "endDeg": 0,
+  "degrees": 0,
+  "index": 15
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "intakeThirdFirstControl",
+    "index": 16
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "intakeThirdSecondControl",
+    "index": 17
+  }
+],
+"color": "#95B5AC"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 70,
+  "heading": "constant",
+  "reverse": false,
+  "name": "scoreThird",
+  "startDeg": 0,
+  "endDeg": 0,
+  "degrees": 0,
+  "index": 18
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 65,
+    "name": "scoreThirdControl",
+    "index": 19
+  }
+],
+"color": "#BA9988"
+},
+{
+  "endPoint": {
+  "x": 9,
+  "y": 34,
+  "heading": "constant",
+  "reverse": false,
+  "name": "intakeFourth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "degrees": 0,
+  "index": 20
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "intakeFourthFirstControl",
+    "index": 21
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "intakeFourthSecondControl",
+    "index": 22
+  }
+],
+"color": "#9A6B66"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 69,
+  "heading": "constant",
+  "reverse": false,
+  "name": "scoreFourth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "degrees": 0,
+  "index": 23
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 63,
+    "name": "scoreFourthControl",
+    "index": 24
+  }
+],
+"color": "#979B8A"
+},
+{
+  "endPoint": {
+  "x": 9,
+  "y": 34,
+  "heading": "constant",
+  "reverse": false,
+  "name": "intakeFifth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "degrees": 0,
+  "index": 25
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "intakeFifthFirstControl",
+    "index": 26
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "intakeFifthSecondControl",
+    "index": 27
+  }
+],
+"color": "#B99CC7"
+},
+{
+  "endPoint": {
+  "x": 42,
+  "y": 68,
+  "heading": "constant",
+  "reverse": false,
+  "name": "scoreFifth",
+  "startDeg": 0,
+  "endDeg": 0,
+  "degrees": 0,
+  "index": 28
+},
+"controlPoints": [
+  {
+    "x": 20,
+    "y": 61,
+    "name": "scoreFifthControl",
+    "index": 29
+  }
+],
+"color": "#BB5765"
+},
+{
+  "endPoint": {
+  "x": 9,
+  "y": 34,
+  "heading": "constant",
+  "reverse": false,
+  "name": "park",
+  "degrees": 0,
+  "index": 30
+},
+"controlPoints": [
+  {
+    "x": 15,
+    "y": 67,
+    "name": "parkControl",
+    "index": 31
+  },
+  {
+    "x": 37,
+    "y": 30,
+    "name": "parkSecondControl",
+    "index": 32
+  }
+],
+"color": "#A7D5B9"
+}
+]
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/OwenMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/DevMain.java
similarity index 81%
rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/OwenMain.java
rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/DevMain.java
index 510d5b6..12a7f9d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/OwenMain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/DevMain.java
@@ -8,36 +8,36 @@
 import org.firstinspires.ftc.teamcode.CommandRobot;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
-@TeleOp(name = "Owen's Main")
-public class OwenMain extends OpModeCore {
+@TeleOp(name = "1 Dev Main")
+public class DevMain extends OpModeCore {
     private CommandRobot robot;
     private MultipleTelemetry multipleTelemetry;
 
     public void initialize() {
+        super.initialize();
+
         this.multipleTelemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
         this.robot = new CommandRobot(
                 super.hardwareMap,
-                this.multipleTelemetry,
                 super.gamepad1,
                 super.gamepad2,
-                this,
-                CommandRobot.TeleOpMode.OWEN
+                CommandRobot.TeleOpMode.DEV
         );
     }
 
     @Override
     public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-
         this.initialize();
         super.waitForStart();
 
-        this.robot.startThreads();
+        this.robot.startThreads(this);
         while (!isStopRequested() && opModeIsActive()) {
             super.resetCycle();
             CommandScheduler.getInstance().run();
 
+            this.robot.update();
+
             super.logCycles();
             super.telemetry.update();
         }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java
index cbd8d26..eebe9f0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/Main.java
@@ -8,36 +8,36 @@
 import org.firstinspires.ftc.teamcode.CommandRobot;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
-@TeleOp(name = "Ryan & Kelly's Main")
+@TeleOp(name = "0 Main")
 public class Main extends OpModeCore {
     private CommandRobot robot;
     private MultipleTelemetry multipleTelemetry;
 
     public void initialize() {
+        super.initialize();
+
         this.multipleTelemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
         this.robot = new CommandRobot(
                 super.hardwareMap,
-                this.multipleTelemetry,
                 super.gamepad1,
                 super.gamepad2,
-                this,
-                CommandRobot.TeleOpMode.RYAN_KELLY
+                CommandRobot.TeleOpMode.KELLY
         );
     }
 
     @Override
     public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-
         this.initialize();
         super.waitForStart();
 
-        this.robot.startThreads();
+        this.robot.startThreads(this);
         while (!isStopRequested() && opModeIsActive()) {
             super.resetCycle();
             CommandScheduler.getInstance().run();
 
+            this.robot.update();
+
             super.logCycles();
             super.telemetry.update();
         }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/RyanMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/RyanMain.java
deleted file mode 100644
index 3452639..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/RyanMain.java
+++ /dev/null
@@ -1,47 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.teleop;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.CommandRobot;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-
-@TeleOp(name = "Ryan's Main")
-public class RyanMain extends OpModeCore {
-    private CommandRobot robot;
-    private MultipleTelemetry multipleTelemetry;
-
-    public void initialize() {
-        this.multipleTelemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        this.robot = new CommandRobot(
-                super.hardwareMap,
-                this.multipleTelemetry,
-                super.gamepad1,
-                super.gamepad2,
-                this,
-                CommandRobot.TeleOpMode.RYAN
-        );
-    }
-
-    @Override
-    public void runOpMode() {
-        CommandScheduler.getInstance().enable();
-
-        this.initialize();
-        super.waitForStart();
-
-        this.robot.startThreads();
-        while (!isStopRequested() && opModeIsActive()) {
-            super.resetCycle();
-            CommandScheduler.getInstance().run();
-
-            super.logCycles();
-            super.telemetry.update();
-        }
-
-        super.end();
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ArmTest.java
new file mode 100644
index 0000000..66c145a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ArmTest.java
@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.opmodes.test;
+
+import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.commands.arm.ArmAccepting;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmBasket;
+import org.firstinspires.ftc.teamcode.commands.arm.ArmReady;
+import org.firstinspires.ftc.teamcode.subsystems.Arm;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
+import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
+
+@TeleOp(name = "Arm Test")
+public class ArmTest extends OpModeCore {
+    private Arm arm;
+    private GamepadEx gamepad;
+
+    @Override
+    public void initialize() {
+        super.initialize();
+
+        this.gamepad = new GamepadEx(super.gamepad1);
+        this.arm = new Arm(super.hardwareMap);
+
+        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ArmAccepting(this.arm));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new ArmReady(this.arm));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new ArmBasket(this.arm));
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        this.initialize();
+
+        super.waitForStart();
+
+        while (opModeIsActive()) {
+            super.resetCycle();
+            CommandScheduler.getInstance().run();
+
+            super.logCycles();
+            TelemetryCore.getInstance().update();
+        }
+
+        super.end();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java
index 46d58bf..3c7d818 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java
@@ -5,38 +5,26 @@
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotAccepting;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotScore;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateDown;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateUp;
-import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoAccepting;
-import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawClose;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawOpen;
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
-import org.firstinspires.ftc.teamcode.subsystems.Extendo;
-import org.firstinspires.ftc.teamcode.utils.commands.GamepadTrigger;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
 @TeleOp(name = "Claw Test")
 public class ClawTest extends OpModeCore {
     private Claw claw;
-    private Extendo extendo;
     private GamepadEx gamepad;
-    private GamepadTrigger intakeAccept, intakeReject;
 
     @Override
     public void initialize() {
-        this.gamepad = new GamepadEx(super.gamepad1);
-        this.claw = new Claw(super.hardwareMap, super.multipleTelemetry);
-        this.extendo = new Extendo(super.hardwareMap, super.multipleTelemetry);
+        super.initialize();
 
-        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ClawRotateDown(super.multipleTelemetry, this.claw));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new ClawRotateDown(super.multipleTelemetry, this.claw));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_LEFT).whenPressed(new ClawRotateUp(super.multipleTelemetry, this.claw));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new ClawPivotScore(super.multipleTelemetry, this.claw));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(new ClawPivotAccepting(super.multipleTelemetry, this.claw));
+        this.gamepad = new GamepadEx(super.gamepad1);
+        this.claw = new Claw(super.hardwareMap);
 
-        this.intakeAccept = new GamepadTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER, d -> this.claw.setClawPower(-d), this.gamepad);
-        this.intakeReject = new GamepadTrigger(GamepadKeys.Trigger.LEFT_TRIGGER, this.claw::setClawPower, this.gamepad);
+        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ClawClose(this.claw));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new ClawOpen(this.claw));
     }
 
     @Override
@@ -51,7 +39,7 @@ public void runOpMode() throws InterruptedException {
             CommandScheduler.getInstance().run();
 
             super.logCycles();
-            super.multipleTelemetry.update();
+            TelemetryCore.getInstance().update();
         }
 
         super.end();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ColorSensorTest.java
deleted file mode 100644
index 8c7f05c..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ColorSensorTest.java
+++ /dev/null
@@ -1,39 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.test;
-
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.ColorRangeSensor;
-
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-import org.firstinspires.ftc.teamcode.utils.hardware.EnhancedColorSensor;
-
-@TeleOp(name = "Color Sensor Test")
-public class ColorSensorTest extends OpModeCore {
-    private EnhancedColorSensor sensor;
-
-    public void initialize() {
-        this.sensor = new EnhancedColorSensor(hardwareMap.get(ColorRangeSensor.class, "sensor"));
-    }
-
-    public void startThreads(OpModeCore opMode) {
-        this.sensor.startThread(opMode);
-    }
-
-    public void runOpMode() throws InterruptedException {
-        this.initialize();
-
-        CommandScheduler.getInstance().enable();
-
-        super.waitForStart();
-
-        this.startThreads(this);
-        while (opModeIsActive()) {
-            super.multipleTelemetry.addData("Distance", this.sensor.getDistance());
-            super.multipleTelemetry.addData("Red", this.sensor.getRed());
-            super.multipleTelemetry.addData("Blue", this.sensor.getBlue());
-
-            super.multipleTelemetry.update();
-        }
-        super.end();
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java
index 48b0b58..695ae4a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/DrivetrainTest.java
@@ -1,11 +1,12 @@
 package org.firstinspires.ftc.teamcode.opmodes.test;
 
-import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.command.CommandScheduler;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.pedropathing.localization.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
 @TeleOp(name = "Drivetrain Test")
@@ -15,14 +16,15 @@ public class DrivetrainTest extends OpModeCore {
 
     @Override
     public void initialize() {
+        super.initialize();
+
         this.gamepad = new GamepadEx(super.gamepad1);
-        this.drive = new Drivetrain(super.hardwareMap, super.multipleTelemetry, new Pose2d(0, 0, 0));
+        this.drive = new Drivetrain(super.hardwareMap, new Pose(0, 0, 0));
     }
 
     @Override
     public void runOpMode() throws InterruptedException {
         this.initialize();
-        CommandScheduler.getInstance().enable();
 
         super.waitForStart();
 
@@ -32,7 +34,7 @@ public void runOpMode() throws InterruptedException {
             CommandScheduler.getInstance().run();
 
             super.logCycles();
-            super.multipleTelemetry.update();
+            TelemetryCore.getInstance().update();
         }
 
         super.end();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java
index 4feab28..7be2338 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ExtendoTest.java
@@ -6,9 +6,10 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoAccepting;
+import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoBasket;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoReady;
-import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
 import org.firstinspires.ftc.teamcode.subsystems.Extendo;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
 @TeleOp(name = "Extendo Test")
@@ -18,18 +19,19 @@ public class ExtendoTest extends OpModeCore {
 
     @Override
     public void initialize() {
+        super.initialize();
+
         this.gamepad = new GamepadEx(super.gamepad1);
-        this.extendo = new Extendo(super.hardwareMap, super.multipleTelemetry);
+        this.extendo = new Extendo(super.hardwareMap);
 
-        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ExtendoAccepting(super.multipleTelemetry, this.extendo));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new ExtendoReady(super.multipleTelemetry, this.extendo));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(new ExtendoScore(super.multipleTelemetry, this.extendo));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ExtendoAccepting(this.extendo));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new ExtendoReady(this.extendo));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(new ExtendoBasket(this.extendo));
     }
 
     @Override
     public void runOpMode() throws InterruptedException {
         this.initialize();
-        CommandScheduler.getInstance().enable();
 
         super.waitForStart();
 
@@ -38,14 +40,14 @@ public void runOpMode() throws InterruptedException {
             super.resetCycle();
             CommandScheduler.getInstance().run();
 
-            super.multipleTelemetry.addData("Position", this.extendo.getPosition());
-            super.multipleTelemetry.addData("Target", this.extendo.getTarget());
-            super.multipleTelemetry.addData("Error", this.extendo.getError());
+            TelemetryCore.getInstance().addData("Position", this.extendo.getPosition());
+            TelemetryCore.getInstance().addData("Target", this.extendo.getTarget());
+            TelemetryCore.getInstance().addData("Error", this.extendo.getError());
 
             this.extendo.setConstants();
 
             super.logCycles();
-            super.multipleTelemetry.update();
+            TelemetryCore.getInstance().update();
         }
 
         super.end();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LiftTest.java
index 1e841e3..3f851d4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LiftTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LiftTest.java
@@ -1,17 +1,18 @@
 package org.firstinspires.ftc.teamcode.opmodes.test;
 
 import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.command.InstantCommand;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.commands.lift.LiftDecrement;
-import org.firstinspires.ftc.teamcode.commands.lift.LiftHighBasket;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftHighRung;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftIncrement;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftLowBasket;
 import org.firstinspires.ftc.teamcode.commands.lift.LiftLowRung;
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
 @TeleOp(name = "Lift Test")
@@ -21,21 +22,22 @@ public class LiftTest extends OpModeCore {
 
     @Override
     public void initialize() {
+        super.initialize();
+
         this.gamepad = new GamepadEx(super.gamepad1);
-        this.lift = new Lift(super.hardwareMap, super.multipleTelemetry);
-
-        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new LiftHighRung(super.multipleTelemetry, this.lift));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new LiftLowRung(super.multipleTelemetry, this.lift));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(new LiftLowBasket(super.multipleTelemetry, this.lift));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new LiftHighBasket(super.multipleTelemetry, this.lift));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(new LiftIncrement(super.multipleTelemetry, this.lift));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(new LiftDecrement(super.multipleTelemetry, this.lift));
+        this.lift = new Lift(super.hardwareMap);
+
+        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new LiftHighRung(this.lift));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new LiftLowRung(this.lift));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(new LiftLowBasket(this.lift));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(new LiftIncrement(this.lift));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(new LiftDecrement(this.lift));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new InstantCommand(() -> this.lift.setTarget(Lift.CLIMB)));
     }
 
     @Override
     public void runOpMode() throws InterruptedException {
         this.initialize();
-        CommandScheduler.getInstance().enable();
 
         super.waitForStart();
 
@@ -44,14 +46,14 @@ public void runOpMode() throws InterruptedException {
             super.resetCycle();
             CommandScheduler.getInstance().run();
 
-            super.multipleTelemetry.addData("Target", this.lift.getTarget());
-            super.multipleTelemetry.addData("Position", this.lift.getPosition());
-            super.multipleTelemetry.addData("Error", this.lift.getError());
+            TelemetryCore.getInstance().addData("Target", this.lift.getTarget());
+            TelemetryCore.getInstance().addData("Position", this.lift.getPosition());
+            TelemetryCore.getInstance().addData("Error", this.lift.getError());
 
             this.lift.setConstants();
 
             super.logCycles();
-            super.multipleTelemetry.update();
+            TelemetryCore.getInstance().update();
         }
 
         super.end();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java
deleted file mode 100644
index 4b5c498..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/LimelightTest.java
+++ /dev/null
@@ -1,76 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.test;
-
-import static org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive.PARAMS;
-
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.qualcomm.hardware.limelightvision.LLResult;
-import com.qualcomm.hardware.limelightvision.LLResultTypes;
-import com.qualcomm.hardware.limelightvision.Limelight3A;
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.IMU;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-import org.firstinspires.ftc.teamcode.utils.hardware.EnhancedIMU;
-
-import java.util.List;
-
-@TeleOp(name = "Limelight Test")
-public class LimelightTest extends OpModeCore {
-    private Limelight3A limelight;
-    // private EnhancedIMU imu;
-
-    @Override
-    public void initialize() {
-        this.limelight = hardwareMap.get(Limelight3A.class, "limelight");
-
-        /* this.imu = new EnhancedIMU(hardwareMap.get(IMU.class, "imu"), new RevHubOrientationOnRobot(
-                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); */
-        // IMU: https://docs.limelightvision.io/docs/docs-limelight/getting-started/FTC/programming#advanced-usage
-
-        this.limelight.pipelineSwitch(0);
-    }
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        this.initialize();
-        CommandScheduler.getInstance().enable();
-
-        this.limelight.start();
-        // this.imu.startThread(this);
-        super.waitForStart();
-        while (opModeIsActive()) {
-            super.resetCycle();
-            CommandScheduler.getInstance().run();
-
-
-            LLResult result = this.limelight.getLatestResult();
-            if (result != null) {
-                if (result.isValid()) {
-                    Pose3D botpose = result.getBotpose();
-                    super.multipleTelemetry.addData("tx", result.getTx());
-                    super.multipleTelemetry.addData("ty", result.getTy());
-
-                    super.multipleTelemetry.addData("pose", botpose.toString());
-                    super.multipleTelemetry.addData("x", botpose.getPosition().x);
-                    super.multipleTelemetry.addData("y", botpose.getPosition().y);
-
-                    List<LLResultTypes.FiducialResult> fiducialResults = result.getFiducialResults();
-                    for (LLResultTypes.FiducialResult fr : fiducialResults) {
-                        super.multipleTelemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees());
-                    }
-
-                    // ...
-                }
-            }
-
-            super.logCycles();
-            super.multipleTelemetry.update();
-        }
-
-        super.end();
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ManualLiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ManualLiftTest.java
deleted file mode 100644
index 25bccbb..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ManualLiftTest.java
+++ /dev/null
@@ -1,51 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmodes.test;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-import com.arcrobotics.ftclib.command.InstantCommand;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.arcrobotics.ftclib.gamepad.GamepadKeys;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.subsystems.Lift;
-import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
-
-@Config
-@TeleOp(name = "Manual Lift Test")
-public class ManualLiftTest extends OpModeCore {
-    public static double POWER;
-    private Lift lift;
-    private GamepadEx gamepad;
-
-    @Override
-    public void initialize() {
-        this.gamepad = new GamepadEx(super.gamepad1);
-        this.lift = new Lift(super.hardwareMap, super.multipleTelemetry);
-
-        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whileHeld(new InstantCommand(() -> this.lift.setPower(POWER)));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whileHeld(new InstantCommand(() -> this.lift.setPower(-POWER)));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenReleased(new InstantCommand(() -> this.lift.setPower(0)));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenReleased(new InstantCommand(() -> this.lift.setPower(0)));
-    }
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        this.initialize();
-        CommandScheduler.getInstance().enable();
-
-        super.waitForStart();
-
-        while (opModeIsActive()) {
-            super.resetCycle();
-            CommandScheduler.getInstance().run();
-
-            super.multipleTelemetry.addData("Position", this.lift.getPosition());
-            super.multipleTelemetry.addLine("FUCK YOU");
-
-            super.logCycles();
-            super.multipleTelemetry.update();
-        }
-
-        super.end();
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/OdometryTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/OdometryTest.java
deleted file mode 100644
index b3757b9..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/OdometryTest.java
+++ /dev/null
@@ -1,201 +0,0 @@
-/*   MIT License
- *   Copyright (c) [2024] [Base 10 Assets, LLC]
- *
- *   Permission is hereby granted, free of charge, to any person obtaining a copy
- *   of this software and associated documentation files (the "Software"), to deal
- *   in the Software without restriction, including without limitation the rights
- *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- *   copies of the Software, and to permit persons to whom the Software is
- *   furnished to do so, subject to the following conditions:
-
- *   The above copyright notice and this permission notice shall be included in all
- *   copies or substantial portions of the Software.
-
- *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- *   SOFTWARE.
- */
-
-package org.firstinspires.ftc.teamcode.opmodes.test;
-
-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.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
-import org.firstinspires.ftc.teamcode.utils.hardware.GoBildaPinpointDriver;
-
-import java.util.Locale;
-
-/*
-This opmode shows how to use the goBILDA® Pinpoint Odometry Computer.
-The goBILDA Odometry Computer is a device designed to solve the Pose Exponential calculation
-commonly associated with Dead Wheel Odometry systems. It reads two encoders, and an integrated
-system of senors to determine the robot's current heading, X position, and Y position.
-
-it uses an ESP32-S3 as a main cpu, with an STM LSM6DSV16X IMU.
-It is validated with goBILDA "Dead Wheel" Odometry pods, but should be compatible with any
-quadrature rotary encoder. The ESP32 PCNT peripheral is speced to decode quadrature encoder signals
-at a maximum of 40mhz per channel. Though the maximum in-application tested number is 130khz.
-
-The device expects two perpendicularly mounted Dead Wheel pods. The encoder pulses are translated
-into mm and their readings are transformed by an "offset", this offset describes how far away
-the pods are from the "tracking point", usually the center of rotation of the robot.
-
-Dead Wheel pods should both increase in count when moved forwards and to the left.
-The gyro will report an increase in heading when rotated counterclockwise.
-
-The Pose Exponential algorithm used is described on pg 181 of this book:
-https://github.com/calcmogul/controls-engineering-in-frc
-
-For support, contact tech@gobilda.com
-
--Ethan Doak
- */
-
-@TeleOp(name = "Odometry Test", group = "Linear OpMode")
-//@Disabled
-
-public class OdometryTest extends LinearOpMode {
-    public final MultipleTelemetry multipleTelemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-
-    GoBildaPinpointDriver odo; // Declare OpMode member for the Odometry Computer
-
-    double oldTime = 0;
-
-
-    @Override
-    public void runOpMode() {
-
-        // Initialize the hardware variables. Note that the strings used here must correspond
-        // to the names assigned during the robot configuration step on the DS or RC devices.
-
-        odo = hardwareMap.get(GoBildaPinpointDriver.class, "odo");
-
-        /*
-        Set the odometry pod positions relative to the point that the odometry computer tracks around.
-        The X pod offset refers to how far sideways from the tracking point the
-        X (forward) odometry pod is. Left of the center is a positive number,
-        right of center is a negative number. the Y pod offset refers to how far forwards from
-        the tracking point the Y (strafe) odometry pod is. forward of center is a positive number,
-        backwards is a negative number.
-         */
-        odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1
-
-        /*
-        Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
-        the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD.
-        If you're using another kind of odometry pod, uncomment setEncoderResolution and input the
-        number of ticks per mm of your odometry pod.
-         */
-        odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
-        //odo.setEncoderResolution(13.26291192);
-
-
-        /*
-        Set the direction that each of the two odometry pods count. The X (forward) pod should
-        increase when you move the robot forward. And the Y (strafe) pod should increase when
-        you move the robot to the left.
-         */
-        odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);
-
-
-        /*
-        Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
-        The IMU will automatically calibrate when first powered on, but recalibrating before running
-        the robot is a good idea to ensure that the calibration is "good".
-        resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
-        This is recommended before you run your autonomous, as a bad initial calibration can cause
-        an incorrect starting value for x, y, and heading.
-         */
-        //odo.recalibrateIMU();
-        odo.resetPosAndIMU();
-
-        multipleTelemetry.addData("Status", "Initialized");
-        multipleTelemetry.addData("X offset", odo.getXOffset());
-        multipleTelemetry.addData("Y offset", odo.getYOffset());
-        multipleTelemetry.addData("Device Version Number:", odo.getDeviceVersion());
-        multipleTelemetry.addData("Device Scalar", odo.getYawScalar());
-        multipleTelemetry.update();
-
-        // Wait for the game to start (driver presses START)
-        waitForStart();
-        resetRuntime();
-
-
-        // run until the end of the match (driver presses STOP)
-        while (opModeIsActive()) {
-
-            /*
-            Request an update from the Pinpoint odometry computer. This checks almost all outputs
-            from the device in a single I2C read.
-             */
-            odo.update();
-
-            /*
-            Optionally, you can update only the heading of the device. This takes less time to read, but will not
-            pull any other data. Only the heading (which you can pull with getHeading() or in getPosition().
-             */
-            //odo.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING);
-
-
-            if (gamepad1.a) {
-                odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU
-            }
-
-            if (gamepad1.b) {
-                odo.recalibrateIMU(); //recalibrates the IMU without resetting position
-            }
-
-            /*
-            This code prints the loop frequency of the REV Control Hub. This frequency is effected
-            by I²C reads/writes. So it's good to keep an eye on. This code calculates the amount
-            of time each cycle takes and finds the frequency (number of updates per second) from
-            that cycle time.
-             */
-            double newTime = getRuntime();
-            double loopTime = newTime - oldTime;
-            double frequency = 1 / loopTime;
-            oldTime = newTime;
-
-
-            /*
-            gets the current Position (x & y in mm, and heading in degrees) of the robot, and prints it.
-             */
-            Pose2D pos = odo.getPosition();
-            String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.INCH), pos.getY(DistanceUnit.INCH), pos.getHeading(AngleUnit.DEGREES));
-            multipleTelemetry.addData("Position", data);
-
-            /*
-            gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and prints it.
-             */
-            Pose2D vel = odo.getVelocity();
-            String velocity = String.format(Locale.US, "{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.getX(DistanceUnit.INCH), vel.getY(DistanceUnit.INCH), vel.getHeading(AngleUnit.DEGREES));
-            multipleTelemetry.addData("Velocity", velocity);
-
-           /*
-            Gets the Pinpoint device status. Pinpoint can reflect a few states. But we'll primarily see
-            READY: the device is working as normal
-            CALIBRATING: the device is calibrating and outputs are put on hold
-            NOT_READY: the device is resetting from scratch. This should only happen after a power-cycle
-            FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in
-            FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in
-            FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in
-            */
-            multipleTelemetry.addData("Status", odo.getDeviceStatus());
-
-            multipleTelemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint
-
-            multipleTelemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate
-            multipleTelemetry.update();
-
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/PivotTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/PivotTest.java
new file mode 100644
index 0000000..1c8b10e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/PivotTest.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.opmodes.test;
+
+import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotAccepting;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotBasket;
+import org.firstinspires.ftc.teamcode.commands.pivot.PivotReady;
+import org.firstinspires.ftc.teamcode.subsystems.Pivot;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
+import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
+
+@TeleOp(name = "Pivot Test")
+public class PivotTest extends OpModeCore {
+    private Pivot pivot;
+    private GamepadEx gamepad;
+
+    @Override
+    public void initialize() {
+        super.initialize();
+
+        this.gamepad = new GamepadEx(super.gamepad1);
+        this.pivot = new Pivot(super.hardwareMap);
+
+        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new PivotAccepting(this.pivot));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new PivotReady(this.pivot));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new PivotBasket(this.pivot));
+
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        this.initialize();
+
+        super.waitForStart();
+
+        while (opModeIsActive()) {
+            super.resetCycle();
+            CommandScheduler.getInstance().run();
+
+            super.logCycles();
+            TelemetryCore.getInstance().update();
+        }
+
+        super.end();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/WristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/WristTest.java
new file mode 100644
index 0000000..b9160f5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/WristTest.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.opmodes.test;
+
+import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.commands.wrist.WristAccepting;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristBasket;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristReady;
+import org.firstinspires.ftc.teamcode.commands.wrist.WristSpecimen;
+import org.firstinspires.ftc.teamcode.subsystems.Wrist;
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
+import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
+
+@TeleOp(name = "Wrist Test")
+public class WristTest extends OpModeCore {
+    private Wrist wrist;
+    private GamepadEx gamepad;
+
+    @Override
+    public void initialize() {
+        super.initialize();
+
+        this.gamepad = new GamepadEx(super.gamepad1);
+        this.wrist = new Wrist(super.hardwareMap);
+
+        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new WristBasket(this.wrist));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new WristReady(this.wrist));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new WristSpecimen(this.wrist));
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        this.initialize();
+
+        super.waitForStart();
+
+        while (opModeIsActive()) {
+            super.resetCycle();
+            CommandScheduler.getInstance().run();
+
+            super.logCycles();
+            TelemetryCore.getInstance().update();
+        }
+
+        super.end();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/constants/FConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/constants/FConstants.java
new file mode 100644
index 0000000..0453ac7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/constants/FConstants.java
@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.constants;
+
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.localization.Localizers;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+public class FConstants {
+    static {
+        FollowerConstants.localizers = Localizers.PINPOINT;
+
+        FollowerConstants.leftFrontMotorName = "leftFront";
+        FollowerConstants.leftRearMotorName = "leftRear";
+        FollowerConstants.rightFrontMotorName = "rightFront";
+        FollowerConstants.rightRearMotorName = "rightRear";
+
+        FollowerConstants.leftFrontMotorDirection = DcMotorSimple.Direction.FORWARD;
+        FollowerConstants.leftRearMotorDirection = DcMotorSimple.Direction.FORWARD;
+        FollowerConstants.rightFrontMotorDirection = DcMotorSimple.Direction.REVERSE;
+        FollowerConstants.rightRearMotorDirection = DcMotorSimple.Direction.REVERSE;
+
+        FollowerConstants.mass = 13.6624119;
+
+        FollowerConstants.xMovement = (75.2569081094134 + 74.47634687810753 + 74.84969114793611 + 74.01989389575301 + 74.68117853957257) / 5;
+        FollowerConstants.yMovement = (51.77359773633608 + 53.33455812427246 + 53.652753295638455 + 54.03180225796051 + 53.616598478594426) / 5;
+
+        FollowerConstants.forwardZeroPowerAcceleration = (-38.15282509154116 + -35.12614067085665 + -34.761979425838284 + -36.43672311260459 + -32.34119403044195) / 5;
+        FollowerConstants.lateralZeroPowerAcceleration = (-65.24094299732764 + -61.25281822719174 + -65.89813061019508 + -64.45869003996229 + -66.29473846726256) / 5;
+
+        FollowerConstants.translationalPIDFCoefficients.setCoefficients(0.35, 0, 0.02, 0);
+        FollowerConstants.useSecondaryTranslationalPID = true;
+        FollowerConstants.secondaryTranslationalPIDFCoefficients.setCoefficients(0.55, 0, 0.03, 0); // Not being used, @see useSecondaryTranslationalPID
+
+        FollowerConstants.headingPIDFCoefficients.setCoefficients(-2.1, 0, -0.15, 0);
+        FollowerConstants.useSecondaryHeadingPID = true;
+        FollowerConstants.secondaryHeadingPIDFCoefficients.setCoefficients(-2.3, 0.01, -0.2, 0); // Not being used, @see useSecondaryHeadingPID
+
+        FollowerConstants.drivePIDFCoefficients.setCoefficients(0.01, 0, 0.0001, 0.6, 0);
+        FollowerConstants.useSecondaryDrivePID = true;
+        FollowerConstants.secondaryDrivePIDFCoefficients.setCoefficients(0.01, 0, 0, 0.6, 0); // Not being used, @see useSecondaryDrivePID
+
+        FollowerConstants.zeroPowerAccelerationMultiplier = 2.5;
+        FollowerConstants.centripetalScaling = -0.0005;
+
+        FollowerConstants.pathEndTimeoutConstraint = 50;
+        FollowerConstants.pathEndTValueConstraint = 0.995;
+        FollowerConstants.pathEndVelocityConstraint = 0.1;
+        FollowerConstants.pathEndTranslationalConstraint = 0.1;
+        FollowerConstants.pathEndHeadingConstraint = 0.007;
+
+        FollowerConstants.useBrakeModeInTeleOp = true;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/constants/LConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/constants/LConstants.java
new file mode 100644
index 0000000..c48bc48
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/constants/LConstants.java
@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.constants;
+
+import com.pedropathing.localization.GoBildaPinpointDriver;
+import com.pedropathing.localization.constants.PinpointConstants;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+public class LConstants {
+    static {
+        PinpointConstants.forwardY = -4.05572598;
+        PinpointConstants.strafeX = 3.31496655354;
+        PinpointConstants.distanceUnit = DistanceUnit.INCH;
+        PinpointConstants.hardwareMapName = "odo";
+        PinpointConstants.useYawScalar = false;
+        PinpointConstants.yawScalar = 1.0;
+        PinpointConstants.useCustomEncoderResolution = false;
+        PinpointConstants.encoderResolution = GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD;
+        PinpointConstants.customEncoderResolution = 13.26291192;
+        PinpointConstants.forwardEncoderDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED;
+        PinpointConstants.strafeEncoderDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/Circle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/Circle.java
new file mode 100644
index 0000000..b06efba
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/Circle.java
@@ -0,0 +1,78 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.examples;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.pathgen.BezierCurve;
+import com.pedropathing.pathgen.PathChain;
+import com.pedropathing.pathgen.Point;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "Circle", group = "Examples")
+public class Circle extends OpMode {
+    private Telemetry telemetryA;
+
+    public static double RADIUS = 10;
+
+    private Follower follower;
+
+    private PathChain circle;
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        follower = new Follower(hardwareMap);
+
+        circle = follower.pathBuilder()
+                .addPath(new BezierCurve(new Point(0, 0, Point.CARTESIAN), new Point(RADIUS, 0, Point.CARTESIAN), new Point(RADIUS, RADIUS, Point.CARTESIAN)))
+                .addPath(new BezierCurve(new Point(RADIUS, RADIUS, Point.CARTESIAN), new Point(RADIUS, 2 * RADIUS, Point.CARTESIAN), new Point(0, 2 * RADIUS, Point.CARTESIAN)))
+                .addPath(new BezierCurve(new Point(0, 2 * RADIUS, Point.CARTESIAN), new Point(-RADIUS, 2 * RADIUS, Point.CARTESIAN), new Point(-RADIUS, RADIUS, Point.CARTESIAN)))
+                .addPath(new BezierCurve(new Point(-RADIUS, RADIUS, Point.CARTESIAN), new Point(-RADIUS, 0, Point.CARTESIAN), new Point(0, 0, Point.CARTESIAN)))
+                .build();
+
+        follower.followPath(circle);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("This will run in a roughly circular shape of radius " + RADIUS
+                + ", starting on the right-most edge. So, make sure you have enough "
+                + "space to the left, front, and back to run the OpMode.");
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(circle);
+        }
+
+        follower.telemetryDebug(telemetryA);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleBucketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleBucketAuto.java
new file mode 100644
index 0000000..c32dcb1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleBucketAuto.java
@@ -0,0 +1,324 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.examples;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.pathgen.BezierCurve;
+import com.pedropathing.pathgen.BezierLine;
+import com.pedropathing.pathgen.Path;
+import com.pedropathing.pathgen.PathChain;
+import com.pedropathing.pathgen.Point;
+import com.pedropathing.util.Constants;
+import com.pedropathing.util.Timer;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is an example auto that showcases movement and control of two servos autonomously.
+ * It is a 0+4 (Specimen + Sample) bucket auto. It scores a neutral preload and then pickups 3 samples from the ground and scores them before parking.
+ * There are examples of different ways to build paths.
+ * A path progression method has been created and can advance based on time, position, or other factors.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 2.0, 11/28/2024
+ */
+
+@Autonomous(name = "Example Auto Blue", group = "Examples")
+public class ExampleBucketAuto extends OpMode {
+
+    private Follower follower;
+    private Timer pathTimer, actionTimer, opmodeTimer;
+
+    /**
+     * This is the variable where we store the state of our auto.
+     * It is used by the pathUpdate method.
+     */
+    private int pathState;
+
+    /* Create and Define Poses + Paths
+     * Poses are built with three constructors: x, y, and heading (in Radians).
+     * Pedro uses 0 - 144 for x and y, with 0, 0 being on the bottom left.
+     * (For Into the Deep, this would be Blue Observation Zone (0,0) to Red Observation Zone (144,144).)
+     * Even though Pedro uses a different coordinate system than RR, you can convert any roadrunner pose by adding +72 both the x and y.
+     * This visualizer is very easy to use to find and create paths/pathchains/poses: <https://pedro-path-generator.vercel.app/>
+     * Lets assume our robot is 18 by 18 inches
+     * Lets assume the Robot is facing the human player and we want to score in the bucket */
+
+    /**
+     * Start Pose of our robot
+     */
+    private final Pose startPose = new Pose(9, 111, Math.toRadians(270));
+
+    /**
+     * Scoring Pose of our robot. It is facing the submersible at a -45 degree (315 degree) angle.
+     */
+    private final Pose scorePose = new Pose(14, 129, Math.toRadians(315));
+
+    /**
+     * Lowest (First) Sample from the Spike Mark
+     */
+    private final Pose pickup1Pose = new Pose(37, 121, Math.toRadians(0));
+
+    /**
+     * Middle (Second) Sample from the Spike Mark
+     */
+    private final Pose pickup2Pose = new Pose(43, 130, Math.toRadians(0));
+
+    /**
+     * Highest (Third) Sample from the Spike Mark
+     */
+    private final Pose pickup3Pose = new Pose(49, 135, Math.toRadians(0));
+
+    /**
+     * Park Pose for our robot, after we do all of the scoring.
+     */
+    private final Pose parkPose = new Pose(60, 98, Math.toRadians(90));
+
+    /**
+     * Park Control Pose for our robot, this is used to manipulate the bezier curve that we will create for the parking.
+     * The Robot will not go to this pose, it is used a control point for our bezier curve.
+     */
+    private final Pose parkControlPose = new Pose(60, 98, Math.toRadians(90));
+
+    /* These are our Paths and PathChains that we will define in buildPaths() */
+    private Path scorePreload, park;
+    private PathChain grabPickup1, grabPickup2, grabPickup3, scorePickup1, scorePickup2, scorePickup3;
+
+    /**
+     * Build the paths for the auto (adds, for example, constant/linear headings while doing paths)
+     * It is necessary to do this so that all the paths are built before the auto starts.
+     **/
+    public void buildPaths() {
+
+        /* There are two major types of paths components: BezierCurves and BezierLines.
+         *    * BezierCurves are curved, and require >= 3 points. There are the start and end points, and the control points.
+         *    - Control points manipulate the curve between the start and end points.
+         *    - A good visualizer for this is [this](https://pedro-path-generator.vercel.app/).
+         *    * BezierLines are straight, and require 2 points. There are the start and end points.
+         * Paths have can have heading interpolation: Constant, Linear, or Tangential
+         *    * Linear heading interpolation:
+         *    - Pedro will slowly change the heading of the robot from the startHeading to the endHeading over the course of the entire path.
+         *    * Constant Heading Interpolation:
+         *    - Pedro will maintain one heading throughout the entire path.
+         *    * Tangential Heading Interpolation:
+         *    - Pedro will follows the angle of the path such that the robot is always driving forward when it follows the path.
+         * PathChains hold Path(s) within it and are able to hold their end point, meaning that they will holdPoint until another path is followed.
+         * Here is a explanation of the difference between Paths and PathChains <https://pedropathing.com/commonissues/pathtopathchain.html> */
+
+        /* This is our scorePreload path. We are using a BezierLine, which is a straight line. */
+        scorePreload = new Path(new BezierLine(new Point(startPose), new Point(scorePose)));
+        scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading());
+
+        /* Here is an example for Constant Interpolation
+        scorePreload.setConstantInterpolation(startPose.getHeading()); */
+
+        /* This is our grabPickup1 PathChain. We are using a single path with a BezierLine, which is a straight line. */
+        grabPickup1 = follower.pathBuilder()
+                .addPath(new BezierLine(new Point(scorePose), new Point(pickup1Pose)))
+                .setLinearHeadingInterpolation(scorePose.getHeading(), pickup1Pose.getHeading())
+                .build();
+
+        /* This is our scorePickup1 PathChain. We are using a single path with a BezierLine, which is a straight line. */
+        scorePickup1 = follower.pathBuilder()
+                .addPath(new BezierLine(new Point(pickup1Pose), new Point(scorePose)))
+                .setLinearHeadingInterpolation(pickup1Pose.getHeading(), scorePose.getHeading())
+                .build();
+
+        /* This is our grabPickup2 PathChain. We are using a single path with a BezierLine, which is a straight line. */
+        grabPickup2 = follower.pathBuilder()
+                .addPath(new BezierLine(new Point(scorePose), new Point(pickup2Pose)))
+                .setLinearHeadingInterpolation(scorePose.getHeading(), pickup2Pose.getHeading())
+                .build();
+
+        /* This is our scorePickup2 PathChain. We are using a single path with a BezierLine, which is a straight line. */
+        scorePickup2 = follower.pathBuilder()
+                .addPath(new BezierLine(new Point(pickup2Pose), new Point(scorePose)))
+                .setLinearHeadingInterpolation(pickup2Pose.getHeading(), scorePose.getHeading())
+                .build();
+
+        /* This is our grabPickup3 PathChain. We are using a single path with a BezierLine, which is a straight line. */
+        grabPickup3 = follower.pathBuilder()
+                .addPath(new BezierLine(new Point(scorePose), new Point(pickup3Pose)))
+                .setLinearHeadingInterpolation(scorePose.getHeading(), pickup3Pose.getHeading())
+                .build();
+
+        /* This is our scorePickup3 PathChain. We are using a single path with a BezierLine, which is a straight line. */
+        scorePickup3 = follower.pathBuilder()
+                .addPath(new BezierLine(new Point(pickup3Pose), new Point(scorePose)))
+                .setLinearHeadingInterpolation(pickup3Pose.getHeading(), scorePose.getHeading())
+                .build();
+
+        /* This is our park path. We are using a BezierCurve with 3 points, which is a curved line that is curved based off of the control point */
+        park = new Path(new BezierCurve(new Point(scorePose), /* Control Point */ new Point(parkControlPose), new Point(parkPose)));
+        park.setLinearHeadingInterpolation(scorePose.getHeading(), parkPose.getHeading());
+    }
+
+    /**
+     * This switch is called continuously and runs the pathing, at certain points, it triggers the action state.
+     * Everytime the switch changes case, it will reset the timer. (This is because of the setPathState() method)
+     * The followPath() function sets the follower to run the specific path, but does NOT wait for it to finish before moving on.
+     */
+    public void autonomousPathUpdate() {
+        switch (pathState) {
+            case 0:
+                follower.followPath(scorePreload);
+                setPathState(1);
+                break;
+            case 1:
+
+                /* You could check for
+                - Follower State: "if(!follower.isBusy() {}" (Though, I don't recommend this because it might not return due to holdEnd
+                - Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}"
+                - Robot Position: "if(follower.getPose().getX() > 36) {}"
+                */
+
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
+                if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) {
+                    /* Score Preload */
+
+                    /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
+                    follower.followPath(grabPickup1, true);
+                    setPathState(2);
+                }
+                break;
+            case 2:
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the pickup1Pose's position */
+                if (follower.getPose().getX() > (pickup1Pose.getX() - 1) && follower.getPose().getY() > (pickup1Pose.getY() - 1)) {
+                    /* Grab Sample */
+
+                    /* Since this is a pathChain, we can have Pedro hold the end point while we are scoring the sample */
+                    follower.followPath(scorePickup1, true);
+                    setPathState(3);
+                }
+                break;
+            case 3:
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
+                if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) {
+                    /* Score Sample */
+
+                    /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
+                    follower.followPath(grabPickup2, true);
+                    setPathState(4);
+                }
+                break;
+            case 4:
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the pickup2Pose's position */
+                if (follower.getPose().getX() > (pickup2Pose.getX() - 1) && follower.getPose().getY() > (pickup2Pose.getY() - 1)) {
+                    /* Grab Sample */
+
+                    /* Since this is a pathChain, we can have Pedro hold the end point while we are scoring the sample */
+                    follower.followPath(scorePickup2, true);
+                    setPathState(5);
+                }
+                break;
+            case 5:
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
+                if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) {
+                    /* Score Sample */
+
+                    /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
+                    follower.followPath(grabPickup3, true);
+                    setPathState(6);
+                }
+                break;
+            case 6:
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the pickup3Pose's position */
+                if (follower.getPose().getX() > (pickup3Pose.getX() - 1) && follower.getPose().getY() > (pickup3Pose.getY() - 1)) {
+                    /* Grab Sample */
+
+                    /* Since this is a pathChain, we can have Pedro hold the end point while we are scoring the sample */
+                    follower.followPath(scorePickup3, true);
+                    setPathState(7);
+                }
+                break;
+            case 7:
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
+                if (follower.getPose().getX() > (scorePose.getX() - 1) && follower.getPose().getY() > (scorePose.getY() - 1)) {
+                    /* Score Sample */
+
+                    /* Since this is a pathChain, we can have Pedro hold the end point while we are parked */
+                    follower.followPath(park, true);
+                    setPathState(8);
+                }
+                break;
+            case 8:
+                /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
+                if (follower.getPose().getX() > (parkPose.getX() - 1) && follower.getPose().getY() > (parkPose.getY() - 1)) {
+                    /* Level 1 Ascent */
+
+                    /* Set the state to a Case we won't use or define, so it just stops running an new paths */
+                    setPathState(-1);
+                }
+                break;
+        }
+    }
+
+    /**
+     * These change the states of the paths and actions
+     * It will also reset the timers of the individual switches
+     **/
+    public void setPathState(int pState) {
+        pathState = pState;
+        pathTimer.resetTimer();
+    }
+
+    /**
+     * This is the main loop of the OpMode, it will run repeatedly after clicking "Play".
+     **/
+    @Override
+    public void loop() {
+
+        // These loop the movements of the robot
+        follower.update();
+        autonomousPathUpdate();
+
+        // Feedback to Driver Hub
+        telemetry.addData("path state", pathState);
+        telemetry.addData("x", follower.getPose().getX());
+        telemetry.addData("y", follower.getPose().getY());
+        telemetry.addData("heading", follower.getPose().getHeading());
+        telemetry.update();
+    }
+
+    /**
+     * This method is called once at the init of the OpMode.
+     **/
+    @Override
+    public void init() {
+        pathTimer = new Timer();
+        opmodeTimer = new Timer();
+        opmodeTimer.resetTimer();
+
+        Constants.setConstants(FConstants.class, LConstants.class);
+        follower = new Follower(hardwareMap);
+        follower.setStartingPose(startPose);
+        buildPaths();
+    }
+
+    /**
+     * This method is called continuously after Init while waiting for "play".
+     **/
+    @Override
+    public void init_loop() {
+    }
+
+    /**
+     * This method is called once at the start of the OpMode.
+     * It runs all the setup actions, including building paths and starting the path system
+     **/
+    @Override
+    public void start() {
+        opmodeTimer.resetTimer();
+        setPathState(0);
+    }
+
+    /**
+     * We do not use this because everything should automatically disable
+     **/
+    @Override
+    public void stop() {
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleFieldCentricTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleFieldCentricTeleop.java
new file mode 100644
index 0000000..c816cca
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleFieldCentricTeleop.java
@@ -0,0 +1,81 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.examples;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is an example teleop that showcases movement and field-centric driving.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 2.0, 12/30/2024
+ */
+
+@TeleOp(name = "Example Field-Centric Teleop", group = "Examples")
+public class ExampleFieldCentricTeleop extends OpMode {
+    private Follower follower;
+    private final Pose startPose = new Pose(0, 0, 0);
+
+    /**
+     * This method is call once when init is played, it initializes the follower
+     **/
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        follower = new Follower(hardwareMap);
+        follower.setStartingPose(startPose);
+    }
+
+    /**
+     * This method is called continuously after Init while waiting to be started.
+     **/
+    @Override
+    public void init_loop() {
+    }
+
+    /**
+     * This method is called once at the start of the OpMode.
+     **/
+    @Override
+    public void start() {
+        follower.startTeleopDrive();
+    }
+
+    /**
+     * This is the main loop of the opmode and runs continuously after play
+     **/
+    @Override
+    public void loop() {
+
+        /* Update Pedro to move the robot based on:
+        - Forward/Backward Movement: -gamepad1.left_stick_y
+        - Left/Right Movement: -gamepad1.left_stick_x
+        - Turn Left/Right Movement: -gamepad1.right_stick_x
+        - Robot-Centric Mode: false
+        */
+
+        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, false);
+        follower.update();
+
+        /* Telemetry Outputs of our Follower */
+        telemetry.addData("X", follower.getPose().getX());
+        telemetry.addData("Y", follower.getPose().getY());
+        telemetry.addData("Heading in Degrees", Math.toDegrees(follower.getPose().getHeading()));
+
+        /* Update Telemetry to the Driver Hub */
+        telemetry.update();
+
+    }
+
+    /**
+     * We do not use this because everything automatically should disable
+     **/
+    @Override
+    public void stop() {
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleRobotCentricTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleRobotCentricTeleop.java
new file mode 100644
index 0000000..9a0aa22
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/ExampleRobotCentricTeleop.java
@@ -0,0 +1,81 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.examples;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is an example teleop that showcases movement and robot-centric driving.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 2.0, 12/30/2024
+ */
+
+@TeleOp(name = "Example Robot-Centric Teleop", group = "Examples")
+public class ExampleRobotCentricTeleop extends OpMode {
+    private Follower follower;
+    private final Pose startPose = new Pose(0, 0, 0);
+
+    /**
+     * This method is call once when init is played, it initializes the follower
+     **/
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        follower = new Follower(hardwareMap);
+        follower.setStartingPose(startPose);
+    }
+
+    /**
+     * This method is called continuously after Init while waiting to be started.
+     **/
+    @Override
+    public void init_loop() {
+    }
+
+    /**
+     * This method is called once at the start of the OpMode.
+     **/
+    @Override
+    public void start() {
+        follower.startTeleopDrive();
+    }
+
+    /**
+     * This is the main loop of the opmode and runs continuously after play
+     **/
+    @Override
+    public void loop() {
+
+        /* Update Pedro to move the robot based on:
+        - Forward/Backward Movement: -gamepad1.left_stick_y
+        - Left/Right Movement: -gamepad1.left_stick_x
+        - Turn Left/Right Movement: -gamepad1.right_stick_x
+        - Robot-Centric Mode: true
+        */
+
+        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true);
+        follower.update();
+
+        /* Telemetry Outputs of our Follower */
+        telemetry.addData("X", follower.getPose().getX());
+        telemetry.addData("Y", follower.getPose().getY());
+        telemetry.addData("Heading in Degrees", Math.toDegrees(follower.getPose().getHeading()));
+
+        /* Update Telemetry to the Driver Hub */
+        telemetry.update();
+
+    }
+
+    /**
+     * We do not use this because everything automatically should disable
+     **/
+    @Override
+    public void stop() {
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/Triangle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/Triangle.java
new file mode 100644
index 0000000..51d9448
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/Triangle.java
@@ -0,0 +1,82 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.examples;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.pathgen.BezierLine;
+import com.pedropathing.pathgen.PathChain;
+import com.pedropathing.pathgen.Point;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+
+/**
+ * This is the Triangle autonomous OpMode.
+ * It runs the robot in a triangle, with the starting point being the bottom-middle point.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge
+ * @version 1.0, 12/30/2024
+ */
+@Autonomous(name = "Triangle", group = "Examples")
+public class Triangle extends OpMode {
+    private Follower follower;
+
+    private final Pose startPose = new Pose(0, 0, Math.toRadians(0));
+    private final Pose interPose = new Pose(24, -24, Math.toRadians(90));
+    private final Pose endPose = new Pose(24, 24, Math.toRadians(45));
+
+    private PathChain triangle;
+
+    private Telemetry telemetryA;
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+
+        if (follower.atParametricEnd()) {
+            follower.followPath(triangle, true);
+        }
+
+        follower.telemetryDebug(telemetryA);
+    }
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "triangle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        follower = new Follower(hardwareMap);
+        follower.setStartingPose(startPose);
+
+        triangle = follower.pathBuilder()
+                .addPath(new BezierLine(new Point(startPose), new Point(interPose)))
+                .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading())
+                .addPath(new BezierLine(new Point(interPose), new Point(endPose)))
+                .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading())
+                .addPath(new BezierLine(new Point(endPose), new Point(startPose)))
+                .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading())
+                .build();
+
+        follower.followPath(triangle);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("This will run in a roughly triangular shape,"
+                + "starting on the bottom-middle point. So, make sure you have enough "
+                + "space to the left, front, and right to run the OpMode.");
+        telemetryA.update();
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardVelocityTuner.java
new file mode 100644
index 0000000..0133a74
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardVelocityTuner.java
@@ -0,0 +1,171 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic;
+
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorName;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.pathgen.MathFunctions;
+import com.pedropathing.pathgen.Vector;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+/**
+ * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max
+ * power until it reaches some specified distance. It records the most recent velocities, and on
+ * reaching the end of the distance, it averages them and prints out the velocity obtained. It is
+ * recommended to run this multiple times on a full battery to get the best results. What this does
+ * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that
+ * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
+ * more accurate following.
+ * You can adjust the distance the robot will travel on FTC Dashboard: 192/168/43/1:8080/dash
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+@Config
+@Autonomous(name = "Forward Velocity Tuner", group = "Automatic Tuners")
+public class ForwardVelocityTuner extends OpMode {
+    private final ArrayList<Double> velocities = new ArrayList<>();
+
+    private DcMotorEx leftFront;
+    private DcMotorEx leftRear;
+    private DcMotorEx rightFront;
+    private DcMotorEx rightRear;
+    private List<DcMotorEx> motors;
+
+    private PoseUpdater poseUpdater;
+
+    public static double DISTANCE = 48;
+    public static double RECORD_NUMBER = 10;
+
+    private Telemetry telemetryA;
+
+    private boolean end;
+
+    /**
+     * This initializes the drive motors as well as the cache of velocities and the FTC Dashboard
+     * telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName);
+        leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
+        rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
+        rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
+
+        motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+
+        for (int i = 0; i < RECORD_NUMBER; i++) {
+            velocities.add(0.0);
+        }
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
+        telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
+        telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
+        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
+        telemetryA.addData("pose", poseUpdater.getPose());
+        telemetryA.update();
+
+    }
+
+    /**
+     * This starts the OpMode by setting the drive motors to run forward at full power.
+     */
+    @Override
+    public void start() {
+        leftFront.setPower(1);
+        leftRear.setPower(1);
+        rightFront.setPower(1);
+        rightRear.setPower(1);
+        end = false;
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on
+     * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent
+     * velocities, and when the robot has run forward enough, these last velocities recorded are
+     * averaged and printed.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
+            requestOpModeStop();
+        }
+
+        poseUpdater.update();
+        if (!end) {
+            if (Math.abs(poseUpdater.getPose().getX()) > DISTANCE) {
+                end = true;
+                for (DcMotorEx motor : motors) {
+                    motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                    motor.setPower(0);
+                }
+            } else {
+                double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, 0)));
+                velocities.add(currentVelocity);
+                velocities.remove(0);
+            }
+        } else {
+            leftFront.setPower(0);
+            leftRear.setPower(0);
+            rightFront.setPower(0);
+            rightRear.setPower(0);
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            }
+            double average = 0;
+            for (Double velocity : velocities) {
+                average += velocity;
+            }
+            average /= velocities.size();
+
+            telemetryA.addData("forward velocity:", average);
+            telemetryA.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardZeroPowerAccelerationTuner.java
new file mode 100644
index 0000000..8239f21
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/ForwardZeroPowerAccelerationTuner.java
@@ -0,0 +1,170 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic;
+
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorName;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.pathgen.MathFunctions;
+import com.pedropathing.pathgen.Vector;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+/**
+ * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot
+ * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting
+ * them to zero power. The deceleration, or negative acceleration, is then measured until the robot
+ * stops. The accelerations across the entire time the robot is slowing down is then averaged and
+ * that number is then printed. This is used to determine how the robot will decelerate in the
+ * forward direction when power is cut, making the estimations used in the calculations for the
+ * drive Vector more accurate and giving better braking at the end of Paths.
+ * You can adjust the max velocity the robot will hit on FTC Dashboard: 192/168/43/1:8080/dash
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+@Config
+@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Automatic Tuners")
+public class ForwardZeroPowerAccelerationTuner extends OpMode {
+    private final ArrayList<Double> accelerations = new ArrayList<>();
+
+    private DcMotorEx leftFront;
+    private DcMotorEx leftRear;
+    private DcMotorEx rightFront;
+    private DcMotorEx rightRear;
+    private List<DcMotorEx> motors;
+
+    private PoseUpdater poseUpdater;
+
+    public static double VELOCITY = 30;
+
+    private double previousVelocity;
+
+    private long previousTimeNano;
+
+    private Telemetry telemetryA;
+
+    private boolean stopping;
+    private boolean end;
+
+    /**
+     * This initializes the drive motors as well as the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName);
+        leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
+        rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
+        rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
+
+        motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("The robot will run forward until it reaches " + VELOCITY + " inches per second.");
+        telemetryA.addLine("Then, it will cut power from the drivetrain and roll to a stop.");
+        telemetryA.addLine("Make sure you have enough room.");
+        telemetryA.addLine("After stopping, the forward zero power acceleration (natural deceleration) will be displayed.");
+        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
+        telemetryA.update();
+    }
+
+    /**
+     * This starts the OpMode by setting the drive motors to run forward at full power.
+     */
+    @Override
+    public void start() {
+        leftFront.setPower(1);
+        leftRear.setPower(1);
+        rightFront.setPower(1);
+        rightRear.setPower(1);
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on
+     * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will
+     * record its deceleration / negative acceleration until it stops. Then, it will average all the
+     * recorded deceleration / negative acceleration and print that value.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
+            requestOpModeStop();
+        }
+
+        poseUpdater.update();
+        Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading());
+        if (!end) {
+            if (!stopping) {
+                if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) {
+                    previousVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading);
+                    previousTimeNano = System.nanoTime();
+                    stopping = true;
+                    for (DcMotorEx motor : motors) {
+                        motor.setPower(0);
+                    }
+                }
+            } else {
+                double currentVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading);
+                accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9)));
+                previousVelocity = currentVelocity;
+                previousTimeNano = System.nanoTime();
+                if (currentVelocity < FollowerConstants.pathEndVelocityConstraint) {
+                    end = true;
+                }
+            }
+        } else {
+            double average = 0;
+            for (Double acceleration : accelerations) {
+                average += acceleration;
+            }
+            average /= accelerations.size();
+
+            telemetryA.addData("forward zero power acceleration (deceleration):", average);
+            telemetryA.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/LateralZeroPowerAccelerationTuner.java
new file mode 100644
index 0000000..893ae27
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/LateralZeroPowerAccelerationTuner.java
@@ -0,0 +1,170 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic;
+
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorName;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.pathgen.MathFunctions;
+import com.pedropathing.pathgen.Vector;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+/**
+ * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot
+ * to the right until a specified velocity is achieved. Then, the robot cuts power to the motors, setting
+ * them to zero power. The deceleration, or negative acceleration, is then measured until the robot
+ * stops. The accelerations across the entire time the robot is slowing down is then averaged and
+ * that number is then printed. This is used to determine how the robot will decelerate in the
+ * forward direction when power is cut, making the estimations used in the calculations for the
+ * drive Vector more accurate and giving better braking at the end of Paths.
+ * You can adjust the max velocity the robot will hit on FTC Dashboard: 192/168/43/1:8080/dash
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+@Config
+@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Automatic Tuners")
+public class LateralZeroPowerAccelerationTuner extends OpMode {
+    private final ArrayList<Double> accelerations = new ArrayList<>();
+
+    private DcMotorEx leftFront;
+    private DcMotorEx leftRear;
+    private DcMotorEx rightFront;
+    private DcMotorEx rightRear;
+    private List<DcMotorEx> motors;
+
+    private PoseUpdater poseUpdater;
+
+    public static double VELOCITY = 30;
+
+    private double previousVelocity;
+
+    private long previousTimeNano;
+
+    private Telemetry telemetryA;
+
+    private boolean stopping;
+    private boolean end;
+
+    /**
+     * This initializes the drive motors as well as the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName);
+        leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
+        rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
+        rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
+
+        motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("The robot will run to the right until it reaches " + VELOCITY + " inches per second.");
+        telemetryA.addLine("Then, it will cut power from the drivetrain and roll to a stop.");
+        telemetryA.addLine("Make sure you have enough room.");
+        telemetryA.addLine("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed.");
+        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
+        telemetryA.update();
+    }
+
+    /**
+     * This starts the OpMode by setting the drive motors to run forward at full power.
+     */
+    @Override
+    public void start() {
+        leftFront.setPower(1);
+        leftRear.setPower(-1);
+        rightFront.setPower(-1);
+        rightRear.setPower(1);
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on
+     * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will
+     * record its deceleration / negative acceleration until it stops. Then, it will average all the
+     * recorded deceleration / negative acceleration and print that value.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
+            requestOpModeStop();
+        }
+
+        poseUpdater.update();
+        Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2);
+        if (!end) {
+            if (!stopping) {
+                if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) {
+                    previousVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading);
+                    previousTimeNano = System.nanoTime();
+                    stopping = true;
+                    for (DcMotorEx motor : motors) {
+                        motor.setPower(0);
+                    }
+                }
+            } else {
+                double currentVelocity = MathFunctions.dotProduct(poseUpdater.getVelocity(), heading);
+                accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9)));
+                previousVelocity = currentVelocity;
+                previousTimeNano = System.nanoTime();
+                if (currentVelocity < FollowerConstants.pathEndVelocityConstraint) {
+                    end = true;
+                }
+            }
+        } else {
+            double average = 0;
+            for (Double acceleration : accelerations) {
+                average += acceleration;
+            }
+            average /= accelerations.size();
+
+            telemetryA.addData("lateral zero power acceleration (deceleration):", average);
+            telemetryA.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/StrafeVelocityTuner.java
new file mode 100644
index 0000000..0239b66
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/automatic/StrafeVelocityTuner.java
@@ -0,0 +1,169 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.automatic;
+
+
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorName;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.pathgen.MathFunctions;
+import com.pedropathing.pathgen.Vector;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+/**
+ * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot right at max
+ * power until it reaches some specified distance. It records the most recent velocities, and on
+ * reaching the end of the distance, it averages them and prints out the velocity obtained. It is
+ * recommended to run this multiple times on a full battery to get the best results. What this does
+ * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that
+ * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
+ * more accurate following.
+ * You can adjust the distance the robot will travel on FTC Dashboard: 192/168/43/1:8080/dash
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+@Config
+@Autonomous(name = "Strafe Velocity Tuner", group = "Automatic Tuners")
+public class StrafeVelocityTuner extends OpMode {
+    private final ArrayList<Double> velocities = new ArrayList<>();
+
+    private DcMotorEx leftFront;
+    private DcMotorEx leftRear;
+    private DcMotorEx rightFront;
+    private DcMotorEx rightRear;
+    private List<DcMotorEx> motors;
+
+    private PoseUpdater poseUpdater;
+
+    public static double DISTANCE = 48;
+    public static double RECORD_NUMBER = 10;
+
+    private Telemetry telemetryA;
+
+    private boolean end;
+
+    /**
+     * This initializes the drive motors as well as the cache of velocities and the FTC Dashboard
+     * telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName);
+        leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
+        rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
+        rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
+
+        motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+
+        for (int i = 0; i < RECORD_NUMBER; i++) {
+            velocities.add(0.0);
+        }
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right.");
+        telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
+        telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity.");
+        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
+        telemetryA.update();
+    }
+
+    /**
+     * This starts the OpMode by setting the drive motors to run right at full power.
+     */
+    @Override
+    public void start() {
+        leftFront.setPower(1);
+        leftRear.setPower(-1);
+        rightFront.setPower(-1);
+        rightRear.setPower(1);
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing CROSS or A on
+     * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent
+     * velocities, and when the robot has run sideways enough, these last velocities recorded are
+     * averaged and printed.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
+            requestOpModeStop();
+        }
+
+        poseUpdater.update();
+        if (!end) {
+            if (Math.abs(poseUpdater.getPose().getY()) > DISTANCE) {
+                end = true;
+                for (DcMotorEx motor : motors) {
+                    motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                    motor.setPower(0);
+                }
+            } else {
+                double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2)));
+                velocities.add(currentVelocity);
+                velocities.remove(0);
+            }
+        } else {
+            leftFront.setPower(0);
+            leftRear.setPower(0);
+            rightFront.setPower(0);
+            rightRear.setPower(0);
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            }
+            double average = 0;
+            for (Double velocity : velocities) {
+                average += velocity;
+            }
+            average /= velocities.size();
+
+            telemetryA.addData("strafe velocity:", average);
+            telemetryA.update();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/ForwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/ForwardTuner.java
new file mode 100644
index 0000000..abfab26
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/ForwardTuner.java
@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.util.Constants;
+import com.pedropathing.util.DashboardPoseTracker;
+import com.pedropathing.util.Drawing;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current distance in ticks to the specified distance in inches. So, to use this, run the
+ * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
+ * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
+ * and average the results. Then, input the multiplier into the forward ticks to inches in your
+ * localizer of choice.
+ * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 5/6/2024
+ */
+@Config
+@Autonomous(name = "Forward Localizer Tuner", group = ".Localization")
+public class ForwardTuner extends OpMode {
+    private PoseUpdater poseUpdater;
+    private DashboardPoseTracker dashboardPoseTracker;
+
+    private Telemetry telemetryA;
+
+    public static double DISTANCE = 48;
+
+    /**
+     * This initializes the PoseUpdater as well as the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        dashboardPoseTracker = new DashboardPoseTracker(poseUpdater);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry.");
+        telemetryA.update();
+
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+
+    /**
+     * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the
+     * calculated multiplier and draws the robot.
+     */
+    @Override
+    public void loop() {
+        poseUpdater.update();
+
+        telemetryA.addData("distance moved", poseUpdater.getPose().getX());
+        telemetryA.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
+        telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getForwardMultiplier()));
+        telemetryA.update();
+
+        Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LateralTuner.java
new file mode 100644
index 0000000..b575cd9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LateralTuner.java
@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.util.Constants;
+import com.pedropathing.util.DashboardPoseTracker;
+import com.pedropathing.util.Drawing;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current distance in ticks to the specified distance in inches. So, to use this, run the
+ * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
+ * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
+ * and average the results. Then, input the multiplier into the strafe ticks to inches in your
+ * localizer of choice.
+ * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 5/6/2024
+ */
+@Config
+@Autonomous(name = "Lateral Localizer Tuner", group = ".Localization")
+public class LateralTuner extends OpMode {
+    private PoseUpdater poseUpdater;
+    private DashboardPoseTracker dashboardPoseTracker;
+
+    private Telemetry telemetryA;
+
+    public static double DISTANCE = 48;
+
+    /**
+     * This initializes the PoseUpdater as well as the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        dashboardPoseTracker = new DashboardPoseTracker(poseUpdater);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry.");
+        telemetryA.update();
+
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+
+    /**
+     * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the
+     * calculated multiplier and draws the robot.
+     */
+    @Override
+    public void loop() {
+        poseUpdater.update();
+
+        telemetryA.addData("distance moved", poseUpdater.getPose().getY());
+        telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
+        telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
+        telemetryA.update();
+
+        Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LocalizationTest.java
new file mode 100644
index 0000000..5dec93f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/LocalizationTest.java
@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization;
+
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.leftRearMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorDirection;
+import static com.pedropathing.follower.FollowerConstants.rightRearMotorName;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.util.Constants;
+import com.pedropathing.util.DashboardPoseTracker;
+import com.pedropathing.util.Drawing;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+import java.util.Arrays;
+import java.util.List;
+
+/**
+ * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a
+ * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot
+ * on FTC Dashboard (192/168/43/1:8080/dash). You should use this to check the robot's localization.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 5/6/2024
+ */
+@Config
+@TeleOp(group = "Teleop Test", name = "Localization Test")
+public class LocalizationTest extends OpMode {
+    private PoseUpdater poseUpdater;
+    private DashboardPoseTracker dashboardPoseTracker;
+    private Telemetry telemetryA;
+
+    private DcMotorEx leftFront;
+    private DcMotorEx leftRear;
+    private DcMotorEx rightFront;
+    private DcMotorEx rightRear;
+    private List<DcMotorEx> motors;
+
+    /**
+     * This initializes the PoseUpdater, the mecanum drive motors, and the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        dashboardPoseTracker = new DashboardPoseTracker(poseUpdater);
+
+        leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName);
+        leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
+        rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
+        rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
+
+        motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("This will print your robot's position to telemetry while "
+                + "allowing robot control through a basic mecanum drive on gamepad 1.");
+        telemetryA.update();
+
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+
+    /**
+     * This updates the robot's pose estimate, the simple mecanum drive, and updates the FTC
+     * Dashboard telemetry with the robot's position as well as draws the robot's position.
+     */
+    @Override
+    public void loop() {
+        poseUpdater.update();
+        dashboardPoseTracker.update();
+
+        double y = -gamepad1.left_stick_y; // Remember, this is reversed!
+        double x = gamepad1.left_stick_x; // this is strafing
+        double rx = gamepad1.right_stick_x;
+
+        // Denominator is the largest motor power (absolute value) or 1
+        // This ensures all the powers maintain the same ratio, but only when
+        // at least one is out of the range [-1, 1]
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+        double leftFrontPower = (y + x + rx) / denominator;
+        double leftRearPower = (y - x + rx) / denominator;
+        double rightFrontPower = (y - x - rx) / denominator;
+        double rightRearPower = (y + x - rx) / denominator;
+
+        leftFront.setPower(leftFrontPower);
+        leftRear.setPower(leftRearPower);
+        rightFront.setPower(rightFrontPower);
+        rightRear.setPower(rightRearPower);
+
+        telemetryA.addData("x", poseUpdater.getPose().getX());
+        telemetryA.addData("y", poseUpdater.getPose().getY());
+        telemetryA.addData("heading", poseUpdater.getPose().getHeading());
+        telemetryA.addData("total heading", poseUpdater.getTotalHeading());
+        telemetryA.update();
+
+        Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/TurnTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/TurnTuner.java
new file mode 100644
index 0000000..441b98c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/localization/TurnTuner.java
@@ -0,0 +1,74 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.localization;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.localization.PoseUpdater;
+import com.pedropathing.util.Constants;
+import com.pedropathing.util.DashboardPoseTracker;
+import com.pedropathing.util.Drawing;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current angle in ticks to the specified angle in radians. So, to use this, run the
+ * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground.
+ * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run
+ * multiple trials and average the results. Then, input the multiplier into the turning ticks to
+ * radians in your localizer of choice.
+ * You can adjust the target angle on FTC Dashboard: 192/168/43/1:8080/dash
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 5/6/2024
+ */
+@Config
+@Autonomous(name = "Turn Localizer Tuner", group = ".Localization")
+public class TurnTuner extends OpMode {
+    private PoseUpdater poseUpdater;
+    private DashboardPoseTracker dashboardPoseTracker;
+
+    private Telemetry telemetryA;
+
+    public static double ANGLE = 2 * Math.PI;
+
+    /**
+     * This initializes the PoseUpdater as well as the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        poseUpdater = new PoseUpdater(hardwareMap);
+
+        dashboardPoseTracker = new DashboardPoseTracker(poseUpdater);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry.");
+        telemetryA.update();
+
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+
+    /**
+     * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the
+     * calculated multiplier and draws the robot.
+     */
+    @Override
+    public void loop() {
+        poseUpdater.update();
+
+        telemetryA.addData("total angle", poseUpdater.getTotalHeading());
+        telemetryA.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians.");
+        telemetryA.addData("multiplier", ANGLE / (poseUpdater.getTotalHeading() / poseUpdater.getLocalizer().getTurningMultiplier()));
+
+        Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
+        Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50");
+        Drawing.sendPacket();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/CurvedBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/CurvedBackAndForth.java
new file mode 100644
index 0000000..50387e1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/CurvedBackAndForth.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.pid;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.pathgen.BezierCurve;
+import com.pedropathing.pathgen.Path;
+import com.pedropathing.pathgen.Point;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is the CurvedBackAndForth autonomous OpMode. It runs the robot in a specified distance
+ * forward and to the left. On reaching the end of the forward Path, the robot runs the backward
+ * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety
+ * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the
+ * centripetal Vector. Remember to test your tunings on StraightBackAndForth as well, since tunings
+ * that work well for curves might have issues going in straight lines.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+@Config
+@Autonomous(name = "Curved Back And Forth", group = "PIDF Testing")
+public class CurvedBackAndForth extends OpMode {
+    private Telemetry telemetryA;
+
+    public static double DISTANCE = 20;
+
+    private boolean forward = true;
+
+    private Follower follower;
+
+    private Path forwards;
+    private Path backwards;
+
+    /**
+     * This initializes the Follower and creates the forward and backward Paths. Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        follower = new Follower(hardwareMap);
+
+        forwards = new Path(new BezierCurve(new Point(0, 0, Point.CARTESIAN), new Point(Math.abs(DISTANCE), 0, Point.CARTESIAN), new Point(Math.abs(DISTANCE), DISTANCE, Point.CARTESIAN)));
+        backwards = new Path(new BezierCurve(new Point(Math.abs(DISTANCE), DISTANCE, Point.CARTESIAN), new Point(Math.abs(DISTANCE), 0, Point.CARTESIAN), new Point(0, 0, Point.CARTESIAN)));
+
+        backwards.setReversed(true);
+
+        follower.followPath(forwards);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("This will run the robot in a curve going " + DISTANCE + " inches"
+                + " to the left and the same number of inches forward. The robot will go"
+                + "forward and backward continuously along the path. Make sure you have"
+                + "enough room.");
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (!follower.isBusy()) {
+            if (forward) {
+                forward = false;
+                follower.followPath(backwards);
+            } else {
+                forward = true;
+                follower.followPath(forwards);
+            }
+        }
+
+        telemetryA.addData("going forward", forward);
+        follower.telemetryDebug(telemetryA);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/StraightBackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/StraightBackAndForth.java
new file mode 100644
index 0000000..e5507b2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuners_tests/pid/StraightBackAndForth.java
@@ -0,0 +1,88 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.tuners_tests.pid;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.pathgen.BezierLine;
+import com.pedropathing.pathgen.Path;
+import com.pedropathing.pathgen.Point;
+import com.pedropathing.util.Constants;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
+
+/**
+ * This is the StraightBackAndForth autonomous OpMode. It runs the robot in a specified distance
+ * straight forward. On reaching the end of the forward Path, the robot runs the backward Path the
+ * same distance back to the start. Rinse and repeat! This is good for testing a variety of Vectors,
+ * like the drive Vector, the translational Vector, and the heading Vector. Remember to test your
+ * tunings on CurvedBackAndForth as well, since tunings that work well for straight lines might
+ * have issues going in curves.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "Straight Back And Forth", group = "PIDF Tuning")
+public class StraightBackAndForth extends OpMode {
+    private Telemetry telemetryA;
+
+    public static double DISTANCE = 40;
+
+    private boolean forward = true;
+
+    private Follower follower;
+
+    private Path forwards;
+    private Path backwards;
+
+    /**
+     * This initializes the Follower and creates the forward and backward Paths. Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        follower = new Follower(hardwareMap);
+
+        forwards = new Path(new BezierLine(new Point(0, 0, Point.CARTESIAN), new Point(DISTANCE, 0, Point.CARTESIAN)));
+        forwards.setConstantHeadingInterpolation(0);
+        backwards = new Path(new BezierLine(new Point(DISTANCE, 0, Point.CARTESIAN), new Point(0, 0, Point.CARTESIAN)));
+        backwards.setConstantHeadingInterpolation(0);
+
+        follower.followPath(forwards);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.addLine("This will run the robot in a straight line going " + DISTANCE
+                + " inches forward. The robot will go forward and backward continuously"
+                + " along the path. Make sure you have enough room.");
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (!follower.isBusy()) {
+            if (forward) {
+                forward = false;
+                follower.followPath(backwards);
+            } else {
+                forward = true;
+                follower.followPath(forwards);
+            }
+        }
+
+        telemetryA.addData("going forward", forward);
+        follower.telemetryDebug(telemetryA);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java
deleted file mode 100644
index 2c9c15a..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java
+++ /dev/null
@@ -1,23 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.Vector2d;
-
-public final class Drawing {
-    private Drawing() {
-    }
-
-
-    public static void drawRobot(Canvas c, Pose2d t) {
-        final double ROBOT_RADIUS = 9;
-
-        c.setStrokeWidth(1);
-        c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
-
-        Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
-        Vector2d p1 = t.position.plus(halfv);
-        Vector2d p2 = p1.plus(halfv);
-        c.strokeLine(p1.x, p1.y, p2.x, p2.y);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java
deleted file mode 100644
index ee7a9f9..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java
+++ /dev/null
@@ -1,8 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.Twist2dDual;
-
-public interface Localizer {
-    Twist2dDual<Time> update();
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java
deleted file mode 100644
index c47eab4..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java
+++ /dev/null
@@ -1,508 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.AccelConstraint;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Actions;
-import com.acmerobotics.roadrunner.AngularVelConstraint;
-import com.acmerobotics.roadrunner.DualNum;
-import com.acmerobotics.roadrunner.HolonomicController;
-import com.acmerobotics.roadrunner.MecanumKinematics;
-import com.acmerobotics.roadrunner.MinVelConstraint;
-import com.acmerobotics.roadrunner.MotorFeedforward;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.Pose2dDual;
-import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.acmerobotics.roadrunner.PoseVelocity2dDual;
-import com.acmerobotics.roadrunner.ProfileAccelConstraint;
-import com.acmerobotics.roadrunner.ProfileParams;
-import com.acmerobotics.roadrunner.Rotation2d;
-import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.TimeTrajectory;
-import com.acmerobotics.roadrunner.TimeTurn;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-import com.acmerobotics.roadrunner.TrajectoryBuilderParams;
-import com.acmerobotics.roadrunner.TurnConstraints;
-import com.acmerobotics.roadrunner.Twist2dDual;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.Vector2dDual;
-import com.acmerobotics.roadrunner.VelConstraint;
-import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
-import com.acmerobotics.roadrunner.ftc.Encoder;
-import com.acmerobotics.roadrunner.ftc.FlightRecorder;
-import com.acmerobotics.roadrunner.ftc.LazyImu;
-import com.acmerobotics.roadrunner.ftc.LynxFirmware;
-import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-import com.acmerobotics.roadrunner.ftc.RawEncoder;
-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.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.IMU;
-import com.qualcomm.robotcore.hardware.VoltageSensor;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.DriveCommandMessage;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.MecanumCommandMessage;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.MecanumLocalizerInputsMessage;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage;
-
-import java.util.Arrays;
-import java.util.LinkedList;
-import java.util.List;
-
-@Config
-public class MecanumDrive {
-    public static class Params {
-        // IMU orientation
-        // TODO: fill in these values based on
-        //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
-        public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
-                RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
-        public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
-                RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
-
-        // drive model parameters
-        public double inPerTick = 1; // If you're using OTOS/Pinpoint leave this at 1 (all values will be in inches, 1 tick = 1 inch)
-        public double lateralInPerTick = 0.5589462073324273; // Tune this with LateralRampLogger (even if you use OTOS/Pinpoint)
-        public double trackWidthTicks = 9.919706591687339;
-
-        // feedforward parameters (in tick units)
-        public double kS = 1.4023140718295783;
-        public double kV = 0.12807128175751004;
-        public double kA = 0.065;
-
-        // path profile parameters (in inches)
-        public double maxWheelVel = 50;
-        public double minProfileAccel = -30;
-        public double maxProfileAccel = 50;
-
-        // turn profile parameters (in radians)
-        public double maxAngVel = Math.PI; // shared with path
-        public double maxAngAccel = Math.PI;
-
-        // path controller gains
-        public double axialGain = 1.0;
-        public double lateralGain = 1.0;
-        public double headingGain = 1.0; // shared with turn
-
-        public double axialVelGain = 0.0;
-        public double lateralVelGain = 0.0;
-        public double headingVelGain = 0.0; // shared with turn
-    }
-
-    public static Params PARAMS = new Params();
-
-    public final MecanumKinematics kinematics = new MecanumKinematics(
-            PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
-
-    public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
-            PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
-    public final VelConstraint defaultVelConstraint =
-            new MinVelConstraint(Arrays.asList(
-                    kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
-                    new AngularVelConstraint(PARAMS.maxAngVel)
-            ));
-    public final AccelConstraint defaultAccelConstraint =
-            new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
-
-    public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
-
-    public final VoltageSensor voltageSensor;
-
-    public LazyImu lazyImu;
-
-    public final Localizer localizer;
-    public Pose2d pose;
-
-    public final LinkedList<Pose2d> poseHistory = new LinkedList<>();
-
-    private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
-    private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
-    private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
-    private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
-
-    public class DriveLocalizer implements Localizer {
-        public final Encoder leftFront, leftBack, rightBack, rightFront;
-        public final IMU imu;
-
-        private double lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
-        private Rotation2d lastHeading;
-        private boolean initialized;
-
-        public DriveLocalizer() {
-            leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
-            leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
-            rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
-            rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
-
-            imu = lazyImu.get();
-
-            // TODO: reverse encoders if needed
-            //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-            leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-            leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
-        }
-
-        @Override
-        public Twist2dDual<Time> update() {
-            PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
-            PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
-            PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
-            PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
-
-            YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
-
-            FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
-                    leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
-
-            Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
-
-            if (!initialized) {
-                initialized = true;
-
-                lastLeftFrontPos = leftFrontPosVel.position;
-                lastLeftBackPos = leftBackPosVel.position;
-                lastRightBackPos = rightBackPosVel.position;
-                lastRightFrontPos = rightFrontPosVel.position;
-
-                lastHeading = heading;
-
-                return new Twist2dDual<>(
-                        Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
-                        DualNum.constant(0.0, 2)
-                );
-            }
-
-            double headingDelta = heading.minus(lastHeading);
-            Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
-                    new DualNum<Time>(new double[]{
-                            (leftFrontPosVel.position - lastLeftFrontPos),
-                            leftFrontPosVel.velocity,
-                    }).times(PARAMS.inPerTick),
-                    new DualNum<Time>(new double[]{
-                            (leftBackPosVel.position - lastLeftBackPos),
-                            leftBackPosVel.velocity,
-                    }).times(PARAMS.inPerTick),
-                    new DualNum<Time>(new double[]{
-                            (rightBackPosVel.position - lastRightBackPos),
-                            rightBackPosVel.velocity,
-                    }).times(PARAMS.inPerTick),
-                    new DualNum<Time>(new double[]{
-                            (rightFrontPosVel.position - lastRightFrontPos),
-                            rightFrontPosVel.velocity,
-                    }).times(PARAMS.inPerTick)
-            ));
-
-            lastLeftFrontPos = leftFrontPosVel.position;
-            lastLeftBackPos = leftBackPosVel.position;
-            lastRightBackPos = rightBackPosVel.position;
-            lastRightFrontPos = rightFrontPosVel.position;
-
-            lastHeading = heading;
-
-            return new Twist2dDual<>(
-                    twist.line,
-                    DualNum.cons(headingDelta, twist.angle.drop(1))
-            );
-        }
-    }
-
-    public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
-        this.pose = pose;
-
-        LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
-
-        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
-            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
-        }
-
-        // TODO: make sure your config has motors with these names (or change them)
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
-        leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
-        rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
-        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
-
-        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
-        // TODO: reverse motor directions if needed
-        //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
-                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
-
-        voltageSensor = hardwareMap.voltageSensor.iterator().next();
-
-        localizer = new DriveLocalizer();
-
-        FlightRecorder.write("MECANUM_PARAMS", PARAMS);
-    }
-
-    public void setDrivePowers(PoseVelocity2d powers) {
-        MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
-                PoseVelocity2dDual.constant(powers, 1));
-
-        double maxPowerMag = 1;
-        for (DualNum<Time> power : wheelVels.all()) {
-            maxPowerMag = Math.max(maxPowerMag, power.value());
-        }
-
-        leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
-        leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
-        rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
-        rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
-    }
-
-    public final class FollowTrajectoryAction implements Action {
-        public final TimeTrajectory timeTrajectory;
-        private double beginTs = -1;
-
-        private final double[] xPoints, yPoints;
-
-        public FollowTrajectoryAction(TimeTrajectory t) {
-            timeTrajectory = t;
-
-            List<Double> disps = com.acmerobotics.roadrunner.Math.range(
-                    0, t.path.length(),
-                    Math.max(2, (int) Math.ceil(t.path.length() / 2)));
-            xPoints = new double[disps.size()];
-            yPoints = new double[disps.size()];
-            for (int i = 0; i < disps.size(); i++) {
-                Pose2d p = t.path.get(disps.get(i), 1).value();
-                xPoints[i] = p.position.x;
-                yPoints[i] = p.position.y;
-            }
-        }
-
-        @Override
-        public boolean run(@NonNull TelemetryPacket p) {
-            double t;
-            if (beginTs < 0) {
-                beginTs = Actions.now();
-                t = 0;
-            } else {
-                t = Actions.now() - beginTs;
-            }
-
-            if (t >= timeTrajectory.duration) {
-                leftFront.setPower(0);
-                leftBack.setPower(0);
-                rightBack.setPower(0);
-                rightFront.setPower(0);
-
-                return false;
-            }
-
-            Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
-            PoseVelocity2d robotVelRobot = updatePoseEstimate();
-
-            PoseVelocity2dDual<Time> command = new HolonomicController(
-                    PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
-                    PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
-            )
-                    .compute(txWorldTarget, pose, robotVelRobot);
-            driveCommandWriter.write(new DriveCommandMessage(command));
-
-            MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
-            double voltage = voltageSensor.getVoltage();
-
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
-                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
-            double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
-            double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
-            double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
-            double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
-            mecanumCommandWriter.write(new MecanumCommandMessage(
-                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
-            ));
-
-            leftFront.setPower(leftFrontPower);
-            leftBack.setPower(leftBackPower);
-            rightBack.setPower(rightBackPower);
-            rightFront.setPower(rightFrontPower);
-
-            p.put("x", pose.position.x);
-            p.put("y", pose.position.y);
-            p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
-
-            Pose2d error = txWorldTarget.value().minusExp(pose);
-            p.put("xError", error.position.x);
-            p.put("yError", error.position.y);
-            p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
-
-            // only draw when active; only one drive action should be active at a time
-            Canvas c = p.fieldOverlay();
-            drawPoseHistory(c);
-
-            c.setStroke("#4CAF50");
-            Drawing.drawRobot(c, txWorldTarget.value());
-
-            c.setStroke("#3F51B5");
-            Drawing.drawRobot(c, pose);
-
-            c.setStroke("#4CAF50FF");
-            c.setStrokeWidth(1);
-            c.strokePolyline(xPoints, yPoints);
-
-            return true;
-        }
-
-        @Override
-        public void preview(Canvas c) {
-            c.setStroke("#4CAF507A");
-            c.setStrokeWidth(1);
-            c.strokePolyline(xPoints, yPoints);
-        }
-    }
-
-    public final class TurnAction implements Action {
-        private final TimeTurn turn;
-
-        private double beginTs = -1;
-
-        public TurnAction(TimeTurn turn) {
-            this.turn = turn;
-        }
-
-        @Override
-        public boolean run(@NonNull TelemetryPacket p) {
-            double t;
-            if (beginTs < 0) {
-                beginTs = Actions.now();
-                t = 0;
-            } else {
-                t = Actions.now() - beginTs;
-            }
-
-            if (t >= turn.duration) {
-                leftFront.setPower(0);
-                leftBack.setPower(0);
-                rightBack.setPower(0);
-                rightFront.setPower(0);
-
-                return false;
-            }
-
-            Pose2dDual<Time> txWorldTarget = turn.get(t);
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
-            PoseVelocity2d robotVelRobot = updatePoseEstimate();
-
-            PoseVelocity2dDual<Time> command = new HolonomicController(
-                    PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
-                    PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
-            )
-                    .compute(txWorldTarget, pose, robotVelRobot);
-            driveCommandWriter.write(new DriveCommandMessage(command));
-
-            MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
-            double voltage = voltageSensor.getVoltage();
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
-                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
-            double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
-            double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
-            double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
-            double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
-            mecanumCommandWriter.write(new MecanumCommandMessage(
-                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
-            ));
-
-            leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
-            leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
-            rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
-            rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
-
-            Canvas c = p.fieldOverlay();
-            drawPoseHistory(c);
-
-            c.setStroke("#4CAF50");
-            Drawing.drawRobot(c, txWorldTarget.value());
-
-            c.setStroke("#3F51B5");
-            Drawing.drawRobot(c, pose);
-
-            c.setStroke("#7C4DFFFF");
-            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
-
-            return true;
-        }
-
-        @Override
-        public void preview(Canvas c) {
-            c.setStroke("#7C4DFF7A");
-            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
-        }
-    }
-
-    public PoseVelocity2d updatePoseEstimate() {
-        Twist2dDual<Time> twist = localizer.update();
-        pose = pose.plus(twist.value());
-
-        poseHistory.add(pose);
-        while (poseHistory.size() > 100) {
-            poseHistory.removeFirst();
-        }
-
-        estimatedPoseWriter.write(new PoseMessage(pose));
-
-        return twist.velocity().value();
-    }
-
-    private void drawPoseHistory(Canvas c) {
-        double[] xPoints = new double[poseHistory.size()];
-        double[] yPoints = new double[poseHistory.size()];
-
-        int i = 0;
-        for (Pose2d t : poseHistory) {
-            xPoints[i] = t.position.x;
-            yPoints[i] = t.position.y;
-
-            i++;
-        }
-
-        c.setStrokeWidth(1);
-        c.setStroke("#3F51B5");
-        c.strokePolyline(xPoints, yPoints);
-    }
-
-    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
-        return new TrajectoryActionBuilder(
-                TurnAction::new,
-                FollowTrajectoryAction::new,
-                new TrajectoryBuilderParams(
-                        1e-6,
-                        new ProfileParams(
-                                0.25, 0.1, 1e-2
-                        )
-                ),
-                beginPose, 0.0,
-                defaultTurnConstraints,
-                defaultVelConstraint, defaultAccelConstraint
-        );
-    }
-
-    public void setPowers(double... powers) {
-        leftFront.setPower(powers[0]);
-        leftBack.setPower(powers[1]);
-        rightBack.setPower(powers[2]);
-        rightFront.setPower(powers[3]);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/PinpointDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/PinpointDrive.java
deleted file mode 100644
index 3b57c24..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/PinpointDrive.java
+++ /dev/null
@@ -1,165 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-
-import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.zyxOrientation;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.acmerobotics.roadrunner.ftc.FlightRecorder;
-import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
-import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR;
-import com.acmerobotics.roadrunner.ftc.LazyImu;
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage;
-
-/**
- * Experimental extension of MecanumDrive that uses the Gobilda Pinpoint sensor for localization.
- * <p>
- * Released under the BSD 3-Clause Clear License by j5155 from 12087 Capital City Dynamics
- * Portions of this code made and released under the MIT License by Gobilda (Base 10 Assets, LLC)
- * Unless otherwise noted, comments are from Gobilda
- */
-@Config
-public class PinpointDrive extends MecanumDrive {
-    public static class Params {
-        /*
-        Set this to the name that your Pinpoint is configured as in your hardware config.
-         */
-        public String pinpointDeviceName = "pinpoint";
-        /*
-        Set the odometry pod positions relative to the point that the odometry computer tracks around.
-        The X pod offset refers to how far sideways from the tracking point the
-        X (forward) odometry pod is. Left of the center is a positive number,
-        right of the center is a negative number. The Y pod offset refers to how far forwards from
-        the tracking point the Y (strafe) odometry pod is: forward of the center is a positive number,
-        backwards is a negative number.
-         */
-        //These are tuned for 3110-0002-0001 Product Insight #1
-        // RR localizer note: These units are inches, presets are converted from mm (which is why they are inexact)
-        public double xOffset = -3.3071;
-        public double yOffset = -6.6142;
-
-        /*
-        Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
-        the goBILDA_SWINGARM_POD or the goBILDA_4_BAR_POD.
-        If you're using another kind of odometry pod, input the number of ticks per millimeter for that pod.
-
-        RR LOCALIZER NOTE: this is ticks per MILLIMETER, NOT inches per tick.
-        This value should be more than one; the value for the Gobilda 4 Bar Pod is approximately 20.
-        To get this value from inPerTick, first convert the value to millimeters (multiply by 25.4)
-        and then take its inverse (one over the value)
-         */
-        public double encoderResolution = GoBildaPinpointDriverRR.goBILDA_4_BAR_POD;
-
-        /*
-        Set the direction that each of the two odometry pods count. The X (forward) pod should
-        increase when you move the robot forward. And the Y (strafe) pod should increase when
-        you move the robot to the left.
-         */
-        public GoBildaPinpointDriver.EncoderDirection xDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED;
-        public GoBildaPinpointDriver.EncoderDirection yDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED;
-
-        /*
-        Use the pinpoint IMU for tuning
-        If true, overrides any IMU setting in MecanumDrive and uses exclusively Pinpoint for tuning
-        You can also use the pinpoint directly in MecanumDrive if this doesn't work for some reason;
-         replace "imu" with "pinpoint" or whatever your pinpoint is called in config.
-         Note: Pinpoint IMU is always used for base localization
-         */
-        public boolean usePinpointIMUForTuning = true;
-    }
-
-    public static Params PARAMS = new Params();
-    public GoBildaPinpointDriverRR pinpoint;
-    private Pose2d lastPinpointPose = pose;
-
-    public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) {
-        super(hardwareMap, pose);
-        FlightRecorder.write("PINPOINT_PARAMS", PARAMS);
-        pinpoint = hardwareMap.get(GoBildaPinpointDriverRR.class, PARAMS.pinpointDeviceName);
-
-        if (PARAMS.usePinpointIMUForTuning) {
-            lazyImu = new LazyImu(hardwareMap, PARAMS.pinpointDeviceName, new RevHubOrientationOnRobot(zyxOrientation(0, 0, 0)));
-        }
-
-        // RR localizer note: don't love this conversion (change driver?)
-        pinpoint.setOffsets(DistanceUnit.MM.fromInches(PARAMS.xOffset), DistanceUnit.MM.fromInches(PARAMS.yOffset));
-
-
-        pinpoint.setEncoderResolution(PARAMS.encoderResolution);
-
-        pinpoint.setEncoderDirections(PARAMS.xDirection, PARAMS.yDirection);
-
-        /*
-        Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
-        The IMU will automatically calibrate when first powered on, but recalibrating before running
-        the robot is a good idea to ensure that the calibration is "good".
-        resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
-        This is recommended before you run your autonomous, as a bad initial calibration can cause
-        an incorrect starting value for x, y, and heading.
-         */
-        //pinpoint.recalibrateIMU();
-        pinpoint.resetPosAndIMU();
-        // wait for pinpoint to finish calibrating
-        try {
-            Thread.sleep(300);
-        } catch (InterruptedException e) {
-            throw new RuntimeException(e);
-        }
-
-        pinpoint.setPosition(pose);
-    }
-
-    @Override
-    public PoseVelocity2d updatePoseEstimate() {
-        if (lastPinpointPose != pose) {
-            // RR localizer note:
-            // Something else is modifying our pose (likely for relocalization),
-            // so we override the sensor's pose with the new pose.
-            // This could potentially cause up to 1 loop worth of drift.
-            // I don't like this solution at all, but it preserves compatibility.
-            // The only alternative is to add getter and setters, but that breaks compat.
-            // Potential alternate solution: timestamp the pose set and backtrack it based on speed?
-            pinpoint.setPosition(pose);
-        }
-        pinpoint.update();
-        pose = pinpoint.getPositionRR();
-        lastPinpointPose = pose;
-
-        // RR standard
-        poseHistory.add(pose);
-        while (poseHistory.size() > 100) {
-            poseHistory.removeFirst();
-        }
-
-        FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
-        FlightRecorder.write("PINPOINT_RAW_POSE", new FTCPoseMessage(pinpoint.getPosition()));
-        FlightRecorder.write("PINPOINT_STATUS", pinpoint.getDeviceStatus());
-
-        return pinpoint.getVelocityRR();
-    }
-
-
-    // for debug logging
-    public static final class FTCPoseMessage {
-        public long timestamp;
-        public double x;
-        public double y;
-        public double heading;
-
-        public FTCPoseMessage(Pose2D pose) {
-            this.timestamp = System.nanoTime();
-            this.x = pose.getX(DistanceUnit.INCH);
-            this.y = pose.getY(DistanceUnit.INCH);
-            this.heading = pose.getHeading(AngleUnit.RADIANS);
-        }
-    }
-
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SensorGoBildaPinpointExample.java
deleted file mode 100644
index 1c6a981..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SensorGoBildaPinpointExample.java
+++ /dev/null
@@ -1,192 +0,0 @@
-/*   MIT License
- *   Copyright (c) [2024] [Base 10 Assets, LLC]
- *
- *   Permission is hereby granted, free of charge, to any person obtaining a copy
- *   of this software and associated documentation files (the "Software"), to deal
- *   in the Software without restriction, including without limitation the rights
- *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- *   copies of the Software, and to permit persons to whom the Software is
- *   furnished to do so, subject to the following conditions:
-
- *   The above copyright notice and this permission notice shall be included in all
- *   copies or substantial portions of the Software.
-
- *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- *   SOFTWARE.
- */
-
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
-import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import java.util.Locale;
-
-/*
-This opmode shows how to use the goBILDA® Pinpoint Odometry Computer.
-The goBILDA Odometry Computer is a device designed to solve the Pose Exponential calculation
-commonly associated with Dead Wheel Odometry systems. It reads two encoders, and an integrated
-system of senors to determine the robot's current heading, X position, and Y position.
-
-it uses an ESP32-S3 as a main cpu, with an STM LSM6DSV16X IMU.
-It is validated with goBILDA "Dead Wheel" Odometry pods, but should be compatible with any
-quadrature rotary encoder. The ESP32 PCNT peripheral is speced to decode quadrature encoder signals
-at a maximum of 40mhz per channel. Though the maximum in-application tested number is 130khz.
-
-The device expects two perpendicularly mounted Dead Wheel pods. The encoder pulses are translated
-into mm and their readings are transformed by an "offset", this offset describes how far away
-the pods are from the "tracking point", usually the center of rotation of the robot.
-
-Dead Wheel pods should both increase in count when moved forwards and to the left.
-The gyro will report an increase in heading when rotated counterclockwise.
-
-The Pose Exponential algorithm used is described on pg 181 of this book:
-https://github.com/calcmogul/controls-engineering-in-frc
-
-For support, contact tech@gobilda.com
-
--Ethan Doak
- */
-
-@TeleOp(name = "goBILDA® PinPoint Odometry Example", group = "Linear OpMode")
-//@Disabled
-public class SensorGoBildaPinpointExample extends LinearOpMode {
-
-    GoBildaPinpointDriverRR odo; // Declare OpMode member for the Odometry Computer
-
-    double oldTime = 0;
-
-
-    @Override
-    public void runOpMode() {
-
-        // Initialize the hardware variables. Note that the strings used here must correspond
-        // to the names assigned during the robot configuration step on the DS or RC devices.
-
-        odo = hardwareMap.get(GoBildaPinpointDriverRR.class, "pinpoint");
-
-        /*
-        Set the odometry pod positions relative to the point that the odometry computer tracks around.
-        The X pod offset refers to how far sideways from the tracking point the
-        X (forward) odometry pod is. Left of the center is a positive number,
-        right of center is a negative number. the Y pod offset refers to how far forwards from
-        the tracking point the Y (strafe) odometry pod is. forward of center is a positive number,
-        backwards is a negative number.
-         */
-        odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1
-
-        /*
-        Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
-        the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD.
-        If you're using another kind of odometry pod, uncomment setEncoderResolution and input the
-        number of ticks per mm of your odometry pod.
-         */
-        odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
-        //odo.setEncoderResolution(13.26291192);
-
-
-        /*
-        Set the direction that each of the two odometry pods count. The X (forward) pod should
-        increase when you move the robot forward. And the Y (strafe) pod should increase when
-        you move the robot to the left.
-         */
-        odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);
-
-
-        /*
-        Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
-        The IMU will automatically calibrate when first powered on, but recalibrating before running
-        the robot is a good idea to ensure that the calibration is "good".
-        resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
-        This is recommended before you run your autonomous, as a bad initial calibration can cause
-        an incorrect starting value for x, y, and heading.
-         */
-        //odo.recalibrateIMU();
-        odo.resetPosAndIMU();
-
-        telemetry.addData("Status", "Initialized");
-        telemetry.addData("X offset", odo.getXOffset());
-        telemetry.addData("Y offset", odo.getYOffset());
-        telemetry.addData("Device Version Number:", odo.getDeviceVersion());
-        telemetry.addData("Device SCalar", odo.getYawScalar());
-        telemetry.update();
-
-        // Wait for the game to start (driver presses START)
-        waitForStart();
-        resetRuntime();
-
-
-        // run until the end of the match (driver presses STOP)
-        while (opModeIsActive()) {
-
-            /*
-            Request a bulk update from the Pinpoint odometry computer. This checks almost all outputs
-            from the device in a single I2C read.
-             */
-            odo.update();
-
-
-            if (gamepad1.a) {
-                odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU
-            }
-
-            if (gamepad1.b) {
-                odo.recalibrateIMU(); //recalibrates the IMU without resetting position
-            }
-
-            /*
-            This code prints the loop frequency of the REV Control Hub. This frequency is effected
-            by I2C reads/writes. So it's good to keep an eye on. This code calculates the amount
-            of time each cycle takes and finds the frequency (number of updates per second) from
-            that cycle time.
-             */
-            double newTime = getRuntime();
-            double loopTime = newTime - oldTime;
-            double frequency = 1 / loopTime;
-            oldTime = newTime;
-
-
-            /*
-            gets the current Position (x & y in inches, and heading in radians) of the robot, and prints it.
-             */
-            Pose2d pos = odo.getPositionRR();
-            String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.position.x, pos.position.y, pos.heading.toDouble());
-            telemetry.addData("Position", data);
-
-
-            /*
-            gets the current Velocity (x & y in inches/sec and heading in radians/sec) and prints it.
-             */
-            PoseVelocity2d vel = odo.getVelocityRR();
-            String velocity = String.format(Locale.US, "{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.linearVel.x, vel.linearVel.y, vel.angVel);
-            telemetry.addData("Velocity", velocity);
-
-            telemetry.addData("X Encoder:", odo.getEncoderX()); //gets the raw data from the X encoder
-            telemetry.addData("Y Encoder:", odo.getEncoderY()); //gets the raw data from the Y encoder
-            telemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint
-
-            /*
-            Gets the Pinpoint device status. Pinpoint can reflect a few states. But we'll primarily see
-            READY: the device is working as normal
-            CALIBRATING: the device is calibrating and outputs are put on hold
-            NOT_READY: the device is resetting from scratch. This should only happen after a power-cycle
-            FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in
-            FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in
-            FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in
-            */
-            telemetry.addData("Status", odo.getDeviceStatus());
-
-            telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate
-            telemetry.update();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SparkFunOTOSDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SparkFunOTOSDrive.java
deleted file mode 100644
index 8999440..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/SparkFunOTOSDrive.java
+++ /dev/null
@@ -1,148 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-
-import static com.acmerobotics.roadrunner.ftc.OTOSKt.OTOSPoseToRRPose;
-import static com.acmerobotics.roadrunner.ftc.OTOSKt.RRPoseToOTOSPose;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
-import com.acmerobotics.roadrunner.ftc.FlightRecorder;
-import com.acmerobotics.roadrunner.ftc.SparkFunOTOSCorrected;
-import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage;
-
-/**
- * Experimental extension of MecanumDrive that uses the SparkFun OTOS sensor for localization.
- * <p>
- * Released under the BSD 3-Clause Clear License by j5155 from 12087 Capital City Dynamics
- * Portions of this code made and released under the MIT License by SparkFun
- * Unless otherwise noted, comments are from SparkFun
- */
-public class SparkFunOTOSDrive extends MecanumDrive {
-    public static class Params {
-        // Assuming you've mounted your sensor to a robot and it's not centered,
-        // you can specify the offset for the sensor relative to the center of the
-        // robot. The units default to inches and degrees, but if you want to use
-        // different units, specify them before setting the offset! Note that as of
-        // firmware version 1.0, these values will be lost after a power cycle, so
-        // you will need to set them each time you power up the sensor. For example, if
-        // the sensor is mounted 5 inches to the left (negative X) and 10 inches
-        // forward (positive Y) of the center of the robot, and mounted 90 degrees
-        // clockwise (negative rotation) from the robot's orientation, the offset
-        // would be {-5, 10, -90}. These can be any value, even the angle can be
-        // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees).
-
-        // RR localizer note: These units are inches and radians.
-        public SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, Math.toRadians(0));
-
-        // Here we can set the linear and angular scalars, which can compensate for
-        // scaling issues with the sensor measurements. Note that as of firmware
-        // version 1.0, these values will be lost after a power cycle, so you will
-        // need to set them each time you power up the sensor. They can be any value
-        // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to
-        // first set both scalars to 1.0, then calibrate the angular scalar, then
-        // the linear scalar. To calibrate the angular scalar, spin the robot by
-        // multiple rotations (eg. 10) to get a precise error, then set the scalar
-        // to the inverse of the error. Remember that the angle wraps from -180 to
-        // 180 degrees, so for example, if after 10 rotations counterclockwise
-        // (positive rotation), the sensor reports -15 degrees, the required scalar
-        // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the
-        // robot a known distance and measure the error; do this multiple times at
-        // multiple speeds to get an average, then set the linear scalar to the
-        // inverse of the error. For example, if you move the robot 100 inches and
-        // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971
-        public double linearScalar = 1.0;
-        public double angularScalar = 1.0;
-    }
-
-    public static SparkFunOTOSDrive.Params PARAMS = new SparkFunOTOSDrive.Params();
-    public SparkFunOTOSCorrected otos;
-    private Pose2d lastOtosPose = pose;
-
-    private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
-
-    public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) {
-        super(hardwareMap, pose);
-        FlightRecorder.write("OTOS_PARAMS", PARAMS);
-        otos = hardwareMap.get(SparkFunOTOSCorrected.class, "sensor_otos");
-        // RR localizer note:
-        // don't change the units, it will stop Dashboard field view from working properly
-        // and might cause various other issues
-        otos.setLinearUnit(DistanceUnit.INCH);
-        otos.setAngularUnit(AngleUnit.RADIANS);
-
-        otos.setOffset(PARAMS.offset);
-        System.out.println("OTOS calibration beginning!");
-        System.out.println(otos.setLinearScalar(PARAMS.linearScalar));
-        System.out.println(otos.setAngularScalar(PARAMS.angularScalar));
-
-        otos.setPosition(RRPoseToOTOSPose(pose));
-        // The IMU on the OTOS includes a gyroscope and accelerometer, which could
-        // have an offset. Note that as of firmware version 1.0, the calibration
-        // will be lost after a power cycle; the OTOS performs a quick calibration
-        // when it powers up, but it is recommended to perform a more thorough
-        // calibration at the start of all your programs. Note that the sensor must
-        // be completely stationary and flat during calibration! When calling
-        // calibrateImu(), you can specify the number of samples to take and whether
-        // to wait until the calibration is complete. If no parameters are provided,
-        // it will take 255 samples and wait until done; each sample takes about
-        // 2.4ms, so about 612ms total
-
-        // RR localizer note: It is technically possible to change the number of samples to slightly reduce init times,
-        // however, I found that it caused pretty severe heading drift.
-        // Also, if you're careful to always wait more than 612ms in init, you could technically disable waitUntilDone;
-        // this would allow your OpMode code to run while the calibration occurs.
-        // However, that may cause other issues.
-        // In the future I hope to do that by default and just add a check in updatePoseEstimate for it
-        System.out.println(otos.calibrateImu(255, true));
-        System.out.println("OTOS calibration complete!");
-    }
-
-    @Override
-    public PoseVelocity2d updatePoseEstimate() {
-        if (lastOtosPose != pose) {
-            // RR localizer note:
-            // Something else is modifying our pose (likely for relocalization),
-            // so we override otos pose with the new pose.
-            // This could potentially cause up to 1 loop worth of drift.
-            // I don't like this solution at all, but it preserves compatibility.
-            // The only alternative is to add getter and setters, but that breaks compat.
-            // Potential alternate solution: timestamp the pose set and backtrack it based on speed?
-            otos.setPosition(RRPoseToOTOSPose(pose));
-        }
-        // RR localizer note:
-        // The values are passed by reference, so we create variables first,
-        // then pass them into the function, then read from them.
-
-        // Reading acceleration worsens loop times by 1ms,
-        // but not reading it would need a custom driver and would break compatibility.
-        // The same is true for speed: we could calculate speed ourselves from pose and time,
-        // but it would be hard, less accurate, and would only save 1ms of loop time.
-        SparkFunOTOS.Pose2D otosPose = new SparkFunOTOS.Pose2D();
-        SparkFunOTOS.Pose2D otosVel = new SparkFunOTOS.Pose2D();
-        SparkFunOTOS.Pose2D otosAcc = new SparkFunOTOS.Pose2D();
-        otos.getPosVelAcc(otosPose, otosVel, otosAcc);
-        pose = OTOSPoseToRRPose(otosPose);
-        lastOtosPose = pose;
-
-        // RR standard
-        poseHistory.add(pose);
-        while (poseHistory.size() > 100) {
-            poseHistory.removeFirst();
-        }
-
-        estimatedPoseWriter.write(new PoseMessage(pose));
-
-        // RR localizer note:
-        // OTOS velocity units happen to be identical to Roadrunners, so we don't need any conversion!
-        return new PoseVelocity2d(new Vector2d(otosVel.x, otosVel.y), otosVel.h);
-    }
-
-
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java
deleted file mode 100644
index 135ee22..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java
+++ /dev/null
@@ -1,497 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.AccelConstraint;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Actions;
-import com.acmerobotics.roadrunner.AngularVelConstraint;
-import com.acmerobotics.roadrunner.Arclength;
-import com.acmerobotics.roadrunner.DualNum;
-import com.acmerobotics.roadrunner.MinVelConstraint;
-import com.acmerobotics.roadrunner.MotorFeedforward;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.Pose2dDual;
-import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.acmerobotics.roadrunner.PoseVelocity2dDual;
-import com.acmerobotics.roadrunner.ProfileAccelConstraint;
-import com.acmerobotics.roadrunner.ProfileParams;
-import com.acmerobotics.roadrunner.RamseteController;
-import com.acmerobotics.roadrunner.TankKinematics;
-import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.TimeTrajectory;
-import com.acmerobotics.roadrunner.TimeTurn;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-import com.acmerobotics.roadrunner.TrajectoryBuilderParams;
-import com.acmerobotics.roadrunner.TurnConstraints;
-import com.acmerobotics.roadrunner.Twist2dDual;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.Vector2dDual;
-import com.acmerobotics.roadrunner.VelConstraint;
-import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
-import com.acmerobotics.roadrunner.ftc.Encoder;
-import com.acmerobotics.roadrunner.ftc.FlightRecorder;
-import com.acmerobotics.roadrunner.ftc.LazyImu;
-import com.acmerobotics.roadrunner.ftc.LynxFirmware;
-import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-import com.acmerobotics.roadrunner.ftc.RawEncoder;
-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.VoltageSensor;
-
-import org.firstinspires.ftc.teamcode.roadrunner.messages.DriveCommandMessage;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.TankCommandMessage;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.TankLocalizerInputsMessage;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collections;
-import java.util.LinkedList;
-import java.util.List;
-
-@Config
-public final class TankDrive {
-    public static class Params {
-        // IMU orientation
-        // TODO: fill in these values based on
-        //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
-        public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
-                RevHubOrientationOnRobot.LogoFacingDirection.UP;
-        public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
-
-        // drive model parameters
-        public double inPerTick = 0;
-        public double trackWidthTicks = 0;
-
-        // feedforward parameters (in tick units)
-        public double kS = 0;
-        public double kV = 0;
-        public double kA = 0;
-
-        // path profile parameters (in inches)
-        public double maxWheelVel = 50;
-        public double minProfileAccel = -30;
-        public double maxProfileAccel = 50;
-
-        // turn profile parameters (in radians)
-        public double maxAngVel = Math.PI; // shared with path
-        public double maxAngAccel = Math.PI;
-
-        // path controller gains
-        public double ramseteZeta = 0.7; // in the range (0, 1)
-        public double ramseteBBar = 2.0; // positive
-
-        // turn controller gains
-        public double turnGain = 0.0;
-        public double turnVelGain = 0.0;
-    }
-
-    public static Params PARAMS = new Params();
-
-    public final TankKinematics kinematics = new TankKinematics(PARAMS.inPerTick * PARAMS.trackWidthTicks);
-
-    public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
-            PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
-    public final VelConstraint defaultVelConstraint =
-            new MinVelConstraint(Arrays.asList(
-                    kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
-                    new AngularVelConstraint(PARAMS.maxAngVel)
-            ));
-    public final AccelConstraint defaultAccelConstraint =
-            new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
-
-    public final List<DcMotorEx> leftMotors, rightMotors;
-
-    public final LazyImu lazyImu;
-
-    public final VoltageSensor voltageSensor;
-
-    public final Localizer localizer;
-    public Pose2d pose;
-
-    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
-
-    private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
-    private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
-    private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
-
-    private final DownsampledWriter tankCommandWriter = new DownsampledWriter("TANK_COMMAND", 50_000_000);
-
-    public class DriveLocalizer implements Localizer {
-        public final List<Encoder> leftEncs, rightEncs;
-
-        private double lastLeftPos, lastRightPos;
-        private boolean initialized;
-
-        public DriveLocalizer() {
-            {
-                List<Encoder> leftEncs = new ArrayList<>();
-                for (DcMotorEx m : leftMotors) {
-                    Encoder e = new OverflowEncoder(new RawEncoder(m));
-                    leftEncs.add(e);
-                }
-                this.leftEncs = Collections.unmodifiableList(leftEncs);
-            }
-
-            {
-                List<Encoder> rightEncs = new ArrayList<>();
-                for (DcMotorEx m : rightMotors) {
-                    Encoder e = new OverflowEncoder(new RawEncoder(m));
-                    rightEncs.add(e);
-                }
-                this.rightEncs = Collections.unmodifiableList(rightEncs);
-            }
-
-            // TODO: reverse encoder directions if needed
-            //   leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
-        }
-
-        @Override
-        public Twist2dDual<Time> update() {
-            List<PositionVelocityPair> leftReadings = new ArrayList<>(), rightReadings = new ArrayList<>();
-            double meanLeftPos = 0.0, meanLeftVel = 0.0;
-            for (Encoder e : leftEncs) {
-                PositionVelocityPair p = e.getPositionAndVelocity();
-                meanLeftPos += p.position;
-                meanLeftVel += p.velocity;
-                leftReadings.add(p);
-            }
-            meanLeftPos /= leftEncs.size();
-            meanLeftVel /= leftEncs.size();
-
-            double meanRightPos = 0.0, meanRightVel = 0.0;
-            for (Encoder e : rightEncs) {
-                PositionVelocityPair p = e.getPositionAndVelocity();
-                meanRightPos += p.position;
-                meanRightVel += p.velocity;
-                rightReadings.add(p);
-            }
-            meanRightPos /= rightEncs.size();
-            meanRightVel /= rightEncs.size();
-
-            FlightRecorder.write("TANK_LOCALIZER_INPUTS",
-                    new TankLocalizerInputsMessage(leftReadings, rightReadings));
-
-            if (!initialized) {
-                initialized = true;
-
-                lastLeftPos = meanLeftPos;
-                lastRightPos = meanRightPos;
-
-                return new Twist2dDual<>(
-                        Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
-                        DualNum.constant(0.0, 2)
-                );
-            }
-
-            TankKinematics.WheelIncrements<Time> twist = new TankKinematics.WheelIncrements<>(
-                    new DualNum<Time>(new double[]{
-                            meanLeftPos - lastLeftPos,
-                            meanLeftVel
-                    }).times(PARAMS.inPerTick),
-                    new DualNum<Time>(new double[]{
-                            meanRightPos - lastRightPos,
-                            meanRightVel,
-                    }).times(PARAMS.inPerTick)
-            );
-
-            lastLeftPos = meanLeftPos;
-            lastRightPos = meanRightPos;
-
-            return kinematics.forward(twist);
-        }
-    }
-
-    public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
-        this.pose = pose;
-
-        LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
-
-        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
-            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
-        }
-
-        // TODO: make sure your config has motors with these names (or change them)
-        //   add additional motors on each side if you have them
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        leftMotors = Collections.singletonList(hardwareMap.get(DcMotorEx.class, "left"));
-        rightMotors = Collections.singletonList(hardwareMap.get(DcMotorEx.class, "right"));
-
-        for (DcMotorEx m : leftMotors) {
-            m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        }
-        for (DcMotorEx m : rightMotors) {
-            m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        }
-
-        // TODO: reverse motor directions if needed
-        //   leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
-
-        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
-                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
-
-        voltageSensor = hardwareMap.voltageSensor.iterator().next();
-
-        localizer = new TankDrive.DriveLocalizer();
-
-        FlightRecorder.write("TANK_PARAMS", PARAMS);
-    }
-
-    public void setDrivePowers(PoseVelocity2d powers) {
-        TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(
-                PoseVelocity2dDual.constant(powers, 1));
-
-        double maxPowerMag = 1;
-        for (DualNum<Time> power : wheelVels.all()) {
-            maxPowerMag = Math.max(maxPowerMag, power.value());
-        }
-
-        for (DcMotorEx m : leftMotors) {
-            m.setPower(wheelVels.left.get(0) / maxPowerMag);
-        }
-        for (DcMotorEx m : rightMotors) {
-            m.setPower(wheelVels.right.get(0) / maxPowerMag);
-        }
-    }
-
-    public final class FollowTrajectoryAction implements Action {
-        public final TimeTrajectory timeTrajectory;
-        private double beginTs = -1;
-
-        private final double[] xPoints, yPoints;
-
-        public FollowTrajectoryAction(TimeTrajectory t) {
-            timeTrajectory = t;
-
-            List<Double> disps = com.acmerobotics.roadrunner.Math.range(
-                    0, t.path.length(),
-                    Math.max(2, (int) Math.ceil(t.path.length() / 2)));
-            xPoints = new double[disps.size()];
-            yPoints = new double[disps.size()];
-            for (int i = 0; i < disps.size(); i++) {
-                Pose2d p = t.path.get(disps.get(i), 1).value();
-                xPoints[i] = p.position.x;
-                yPoints[i] = p.position.y;
-            }
-        }
-
-        @Override
-        public boolean run(@NonNull TelemetryPacket p) {
-            double t;
-            if (beginTs < 0) {
-                beginTs = Actions.now();
-                t = 0;
-            } else {
-                t = Actions.now() - beginTs;
-            }
-
-            if (t >= timeTrajectory.duration) {
-                for (DcMotorEx m : leftMotors) {
-                    m.setPower(0);
-                }
-                for (DcMotorEx m : rightMotors) {
-                    m.setPower(0);
-                }
-
-                return false;
-            }
-
-            DualNum<Time> x = timeTrajectory.profile.get(t);
-
-            Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
-            updatePoseEstimate();
-
-            PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, PARAMS.ramseteZeta, PARAMS.ramseteBBar)
-                    .compute(x, txWorldTarget, pose);
-            driveCommandWriter.write(new DriveCommandMessage(command));
-
-            TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
-            double voltage = voltageSensor.getVoltage();
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
-                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
-            double leftPower = feedforward.compute(wheelVels.left) / voltage;
-            double rightPower = feedforward.compute(wheelVels.right) / voltage;
-            tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
-
-            for (DcMotorEx m : leftMotors) {
-                m.setPower(leftPower);
-            }
-            for (DcMotorEx m : rightMotors) {
-                m.setPower(rightPower);
-            }
-
-            p.put("x", pose.position.x);
-            p.put("y", pose.position.y);
-            p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
-
-            Pose2d error = txWorldTarget.value().minusExp(pose);
-            p.put("xError", error.position.x);
-            p.put("yError", error.position.y);
-            p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
-
-            // only draw when active; only one drive action should be active at a time
-            Canvas c = p.fieldOverlay();
-            drawPoseHistory(c);
-
-            c.setStroke("#4CAF50");
-            Drawing.drawRobot(c, txWorldTarget.value());
-
-            c.setStroke("#3F51B5");
-            Drawing.drawRobot(c, pose);
-
-            c.setStroke("#4CAF50FF");
-            c.setStrokeWidth(1);
-            c.strokePolyline(xPoints, yPoints);
-
-            return true;
-        }
-
-        @Override
-        public void preview(Canvas c) {
-            c.setStroke("#4CAF507A");
-            c.setStrokeWidth(1);
-            c.strokePolyline(xPoints, yPoints);
-        }
-    }
-
-    public final class TurnAction implements Action {
-        private final TimeTurn turn;
-
-        private double beginTs = -1;
-
-        public TurnAction(TimeTurn turn) {
-            this.turn = turn;
-        }
-
-        @Override
-        public boolean run(@NonNull TelemetryPacket p) {
-            double t;
-            if (beginTs < 0) {
-                beginTs = Actions.now();
-                t = 0;
-            } else {
-                t = Actions.now() - beginTs;
-            }
-
-            if (t >= turn.duration) {
-                for (DcMotorEx m : leftMotors) {
-                    m.setPower(0);
-                }
-                for (DcMotorEx m : rightMotors) {
-                    m.setPower(0);
-                }
-
-                return false;
-            }
-
-            Pose2dDual<Time> txWorldTarget = turn.get(t);
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
-            PoseVelocity2d robotVelRobot = updatePoseEstimate();
-
-            PoseVelocity2dDual<Time> command = new PoseVelocity2dDual<>(
-                    Vector2dDual.constant(new Vector2d(0, 0), 3),
-                    txWorldTarget.heading.velocity().plus(
-                            PARAMS.turnGain * pose.heading.minus(txWorldTarget.heading.value()) +
-                                    PARAMS.turnVelGain * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
-                    )
-            );
-            driveCommandWriter.write(new DriveCommandMessage(command));
-
-            TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
-            double voltage = voltageSensor.getVoltage();
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
-                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
-            double leftPower = feedforward.compute(wheelVels.left) / voltage;
-            double rightPower = feedforward.compute(wheelVels.right) / voltage;
-            tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
-
-            for (DcMotorEx m : leftMotors) {
-                m.setPower(leftPower);
-            }
-            for (DcMotorEx m : rightMotors) {
-                m.setPower(rightPower);
-            }
-
-            Canvas c = p.fieldOverlay();
-            drawPoseHistory(c);
-
-            c.setStroke("#4CAF50");
-            Drawing.drawRobot(c, txWorldTarget.value());
-
-            c.setStroke("#3F51B5");
-            Drawing.drawRobot(c, pose);
-
-            c.setStroke("#7C4DFFFF");
-            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
-
-            return true;
-        }
-
-        @Override
-        public void preview(Canvas c) {
-            c.setStroke("#7C4DFF7A");
-            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
-        }
-    }
-
-    public PoseVelocity2d updatePoseEstimate() {
-        Twist2dDual<Time> twist = localizer.update();
-        pose = pose.plus(twist.value());
-
-        poseHistory.add(pose);
-        while (poseHistory.size() > 100) {
-            poseHistory.removeFirst();
-        }
-
-        estimatedPoseWriter.write(new PoseMessage(pose));
-
-        return twist.velocity().value();
-    }
-
-    private void drawPoseHistory(Canvas c) {
-        double[] xPoints = new double[poseHistory.size()];
-        double[] yPoints = new double[poseHistory.size()];
-
-        int i = 0;
-        for (Pose2d t : poseHistory) {
-            xPoints[i] = t.position.x;
-            yPoints[i] = t.position.y;
-
-            i++;
-        }
-
-        c.setStrokeWidth(1);
-        c.setStroke("#3F51B5");
-        c.strokePolyline(xPoints, yPoints);
-    }
-
-    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
-        return new TrajectoryActionBuilder(
-                TurnAction::new,
-                FollowTrajectoryAction::new,
-                new TrajectoryBuilderParams(
-                        1e-6,
-                        new ProfileParams(
-                                0.25, 0.1, 1e-2
-                        )
-                ),
-                beginPose, 0.0,
-                defaultTurnConstraints,
-                defaultVelConstraint, defaultAccelConstraint
-        );
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.java
deleted file mode 100644
index b9652b3..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.java
+++ /dev/null
@@ -1,99 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.DualNum;
-import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.Twist2dDual;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.Vector2dDual;
-import com.acmerobotics.roadrunner.ftc.Encoder;
-import com.acmerobotics.roadrunner.ftc.FlightRecorder;
-import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-import com.acmerobotics.roadrunner.ftc.RawEncoder;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.teamcode.roadrunner.messages.ThreeDeadWheelInputsMessage;
-
-@Config
-public final class ThreeDeadWheelLocalizer implements Localizer {
-    public static class Params {
-        public double par0YTicks = 0.0; // y position of the first parallel encoder (in tick units)
-        public double par1YTicks = 1.0; // y position of the second parallel encoder (in tick units)
-        public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
-    }
-
-    public static Params PARAMS = new Params();
-
-    public final Encoder par0, par1, perp;
-
-    public final double inPerTick;
-
-    private double lastPar0Pos, lastPar1Pos, lastPerpPos;
-    private boolean initialized;
-
-    public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
-        // TODO: make sure your config has **motors** with these names (or change them)
-        //   the encoders should be plugged into the slot matching the named motor
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0")));
-        par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1")));
-        perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
-
-        // TODO: reverse encoder directions if needed
-        //   par0.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        this.inPerTick = inPerTick;
-
-        FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS);
-    }
-
-    public Twist2dDual<Time> update() {
-        PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
-        PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
-        PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
-
-        FlightRecorder.write("THREE_DEAD_WHEEL_INPUTS", new ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel));
-
-        if (!initialized) {
-            initialized = true;
-
-            lastPar0Pos = par0PosVel.position;
-            lastPar1Pos = par1PosVel.position;
-            lastPerpPos = perpPosVel.position;
-
-            return new Twist2dDual<>(
-                    Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
-                    DualNum.constant(0.0, 2)
-            );
-        }
-
-        double par0PosDelta = par0PosVel.position - lastPar0Pos;
-        double par1PosDelta = par1PosVel.position - lastPar1Pos;
-        double perpPosDelta = perpPosVel.position - lastPerpPos;
-
-        Twist2dDual<Time> twist = new Twist2dDual<>(
-                new Vector2dDual<>(
-                        new DualNum<Time>(new double[]{
-                                (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                                (PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                        }).times(inPerTick),
-                        new DualNum<Time>(new double[]{
-                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
-                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
-                        }).times(inPerTick)
-                ),
-                new DualNum<>(new double[]{
-                        (par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                        (par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                })
-        );
-
-        lastPar0Pos = par0PosVel.position;
-        lastPar1Pos = par1PosVel.position;
-        lastPerpPos = perpPosVel.position;
-
-        return twist;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TwoDeadWheelLocalizer.java
deleted file mode 100644
index 1197fa9..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TwoDeadWheelLocalizer.java
+++ /dev/null
@@ -1,129 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.DualNum;
-import com.acmerobotics.roadrunner.Rotation2d;
-import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.Twist2dDual;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.Vector2dDual;
-import com.acmerobotics.roadrunner.ftc.Encoder;
-import com.acmerobotics.roadrunner.ftc.FlightRecorder;
-import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-import com.acmerobotics.roadrunner.ftc.RawEncoder;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.IMU;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
-import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-import org.firstinspires.ftc.teamcode.roadrunner.messages.TwoDeadWheelInputsMessage;
-
-@Config
-public final class TwoDeadWheelLocalizer implements Localizer {
-    public static class Params {
-        public double parYTicks = 0.0; // y position of the parallel encoder (in tick units)
-        public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
-    }
-
-    public static Params PARAMS = new Params();
-
-    public final Encoder par, perp;
-    public final IMU imu;
-
-    private double lastParPos, lastPerpPos;
-    private Rotation2d lastHeading;
-
-    private final double inPerTick;
-
-    private double lastRawHeadingVel, headingVelOffset;
-    private boolean initialized;
-
-    public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) {
-        // TODO: make sure your config has **motors** with these names (or change them)
-        //   the encoders should be plugged into the slot matching the named motor
-        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));
-        perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
-
-        // TODO: reverse encoder directions if needed
-        //   par.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        this.imu = imu;
-
-        this.inPerTick = inPerTick;
-
-        FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS);
-    }
-
-    public Twist2dDual<Time> update() {
-        PositionVelocityPair parPosVel = par.getPositionAndVelocity();
-        PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
-
-        YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
-        // Use degrees here to work around https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1070
-        AngularVelocity angularVelocityDegrees = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
-        AngularVelocity angularVelocity = new AngularVelocity(
-                UnnormalizedAngleUnit.RADIANS,
-                (float) Math.toRadians(angularVelocityDegrees.xRotationRate),
-                (float) Math.toRadians(angularVelocityDegrees.yRotationRate),
-                (float) Math.toRadians(angularVelocityDegrees.zRotationRate),
-                angularVelocityDegrees.acquisitionTime
-        );
-
-        FlightRecorder.write("TWO_DEAD_WHEEL_INPUTS", new TwoDeadWheelInputsMessage(parPosVel, perpPosVel, angles, angularVelocity));
-
-        Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
-
-        // see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
-        double rawHeadingVel = angularVelocity.zRotationRate;
-        if (Math.abs(rawHeadingVel - lastRawHeadingVel) > Math.PI) {
-            headingVelOffset -= Math.signum(rawHeadingVel) * 2 * Math.PI;
-        }
-        lastRawHeadingVel = rawHeadingVel;
-        double headingVel = headingVelOffset + rawHeadingVel;
-
-        if (!initialized) {
-            initialized = true;
-
-            lastParPos = parPosVel.position;
-            lastPerpPos = perpPosVel.position;
-            lastHeading = heading;
-
-            return new Twist2dDual<>(
-                    Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
-                    DualNum.constant(0.0, 2)
-            );
-        }
-
-        double parPosDelta = parPosVel.position - lastParPos;
-        double perpPosDelta = perpPosVel.position - lastPerpPos;
-        double headingDelta = heading.minus(lastHeading);
-
-        Twist2dDual<Time> twist = new Twist2dDual<>(
-                new Vector2dDual<>(
-                        new DualNum<Time>(new double[]{
-                                parPosDelta - PARAMS.parYTicks * headingDelta,
-                                parPosVel.velocity - PARAMS.parYTicks * headingVel,
-                        }).times(inPerTick),
-                        new DualNum<Time>(new double[]{
-                                perpPosDelta - PARAMS.perpXTicks * headingDelta,
-                                perpPosVel.velocity - PARAMS.perpXTicks * headingVel,
-                        }).times(inPerTick)
-                ),
-                new DualNum<>(new double[]{
-                        headingDelta,
-                        headingVel,
-                })
-        );
-
-        lastParPos = parPosVel.position;
-        lastPerpPos = perpPosVel.position;
-        lastHeading = heading;
-
-        return twist;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.java
deleted file mode 100644
index 4bb4633..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.java
+++ /dev/null
@@ -1,24 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-import com.acmerobotics.roadrunner.PoseVelocity2dDual;
-import com.acmerobotics.roadrunner.Time;
-
-public final class DriveCommandMessage {
-    public long timestamp;
-    public double forwardVelocity;
-    public double forwardAcceleration;
-    public double lateralVelocity;
-    public double lateralAcceleration;
-    public double angularVelocity;
-    public double angularAcceleration;
-
-    public DriveCommandMessage(PoseVelocity2dDual<Time> poseVelocity) {
-        this.timestamp = System.nanoTime();
-        this.forwardVelocity = poseVelocity.linearVel.x.get(0);
-        this.forwardAcceleration = poseVelocity.linearVel.x.get(1);
-        this.lateralVelocity = poseVelocity.linearVel.y.get(0);
-        this.lateralAcceleration = poseVelocity.linearVel.y.get(1);
-        this.angularVelocity = poseVelocity.angVel.get(0);
-        this.angularAcceleration = poseVelocity.angVel.get(1);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.java
deleted file mode 100644
index c8668c2..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.java
+++ /dev/null
@@ -1,19 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-public final class MecanumCommandMessage {
-    public long timestamp;
-    public double voltage;
-    public double leftFrontPower;
-    public double leftBackPower;
-    public double rightBackPower;
-    public double rightFrontPower;
-
-    public MecanumCommandMessage(double voltage, double leftFrontPower, double leftBackPower, double rightBackPower, double rightFrontPower) {
-        this.timestamp = System.nanoTime();
-        this.voltage = voltage;
-        this.leftFrontPower = leftFrontPower;
-        this.leftBackPower = leftBackPower;
-        this.rightBackPower = rightBackPower;
-        this.rightFrontPower = rightFrontPower;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.java
deleted file mode 100644
index ccfe61f..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-
-public final class MecanumLocalizerInputsMessage {
-    public long timestamp;
-    public PositionVelocityPair leftFront;
-    public PositionVelocityPair leftBack;
-    public PositionVelocityPair rightBack;
-    public PositionVelocityPair rightFront;
-    public double yaw;
-    public double pitch;
-    public double roll;
-
-    public MecanumLocalizerInputsMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront, YawPitchRollAngles angles) {
-        this.timestamp = System.nanoTime();
-        this.leftFront = leftFront;
-        this.leftBack = leftBack;
-        this.rightBack = rightBack;
-        this.rightFront = rightFront;
-        {
-            this.yaw = angles.getYaw(AngleUnit.RADIANS);
-            this.pitch = angles.getPitch(AngleUnit.RADIANS);
-            this.roll = angles.getRoll(AngleUnit.RADIANS);
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.java
deleted file mode 100644
index aa652f1..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.java
+++ /dev/null
@@ -1,18 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-import com.acmerobotics.roadrunner.Pose2d;
-
-public final class PoseMessage {
-    public long timestamp;
-    public double x;
-    public double y;
-    public double heading;
-
-    public PoseMessage(Pose2d pose) {
-        this.timestamp = System.nanoTime();
-        this.x = pose.position.x;
-        this.y = pose.position.y;
-        this.heading = pose.heading.toDouble();
-    }
-}
-
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.java
deleted file mode 100644
index 20245b1..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.java
+++ /dev/null
@@ -1,15 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-public final class TankCommandMessage {
-    public long timestamp;
-    public double voltage;
-    public double leftPower;
-    public double rightPower;
-
-    public TankCommandMessage(double voltage, double leftPower, double rightPower) {
-        this.timestamp = System.nanoTime();
-        this.voltage = voltage;
-        this.leftPower = leftPower;
-        this.rightPower = rightPower;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.java
deleted file mode 100644
index a0d5a51..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.java
+++ /dev/null
@@ -1,17 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-
-import java.util.List;
-
-public final class TankLocalizerInputsMessage {
-    public long timestamp;
-    public PositionVelocityPair[] left;
-    public PositionVelocityPair[] right;
-
-    public TankLocalizerInputsMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) {
-        this.timestamp = System.nanoTime();
-        this.left = left.toArray(new PositionVelocityPair[0]);
-        this.right = right.toArray(new PositionVelocityPair[0]);
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.java
deleted file mode 100644
index f376486..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.java
+++ /dev/null
@@ -1,17 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-
-public final class ThreeDeadWheelInputsMessage {
-    public long timestamp;
-    public PositionVelocityPair par0;
-    public PositionVelocityPair par1;
-    public PositionVelocityPair perp;
-
-    public ThreeDeadWheelInputsMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) {
-        this.timestamp = System.nanoTime();
-        this.par0 = par0;
-        this.par1 = par1;
-        this.perp = perp;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.java
deleted file mode 100644
index 27b3d42..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.java
+++ /dev/null
@@ -1,35 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.messages;
-
-import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-
-public final class TwoDeadWheelInputsMessage {
-    public long timestamp;
-    public PositionVelocityPair par;
-    public PositionVelocityPair perp;
-    public double yaw;
-    public double pitch;
-    public double roll;
-    public double xRotationRate;
-    public double yRotationRate;
-    public double zRotationRate;
-
-    public TwoDeadWheelInputsMessage(PositionVelocityPair par, PositionVelocityPair perp, YawPitchRollAngles angles, AngularVelocity angularVelocity) {
-        this.timestamp = System.nanoTime();
-        this.par = par;
-        this.perp = perp;
-        {
-            this.yaw = angles.getYaw(AngleUnit.RADIANS);
-            this.pitch = angles.getPitch(AngleUnit.RADIANS);
-            this.roll = angles.getRoll(AngleUnit.RADIANS);
-        }
-        {
-            this.xRotationRate = angularVelocity.xRotationRate;
-            this.yRotationRate = angularVelocity.yRotationRate;
-            this.zRotationRate = angularVelocity.zRotationRate;
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.java
deleted file mode 100644
index 873fddd..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.java
+++ /dev/null
@@ -1,45 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.tuning;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.roadrunner.Drawing;
-import org.firstinspires.ftc.teamcode.roadrunner.PinpointDrive;
-
-public class LocalizationTest extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        PinpointDrive drive = new PinpointDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-        waitForStart();
-
-        while (opModeIsActive()) {
-            drive.setDrivePowers(new PoseVelocity2d(
-                    new Vector2d(
-                            -gamepad1.left_stick_y,
-                            -gamepad1.left_stick_x
-                    ),
-                    -gamepad1.right_stick_x
-            ));
-
-            drive.updatePoseEstimate();
-
-            telemetry.addData("x", drive.pose.position.x);
-            telemetry.addData("y", drive.pose.position.y);
-            telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble()));
-            telemetry.update();
-
-            TelemetryPacket packet = new TelemetryPacket();
-            packet.fieldOverlay().setStroke("#3F51B5");
-            Drawing.drawRobot(packet.fieldOverlay(), drive.pose);
-            FtcDashboard.getInstance().sendTelemetryPacket(packet);
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.java
deleted file mode 100644
index 5a9607e..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.java
+++ /dev/null
@@ -1,89 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.tuning;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.ftc.Actions;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.PinpointDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.SparkFunOTOSDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.TankDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer;
-import org.firstinspires.ftc.teamcode.roadrunner.TwoDeadWheelLocalizer;
-
-public final class ManualFeedbackTuner extends LinearOpMode {
-    public static double DISTANCE = 64;
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        if (TuningOpModes.DRIVE_CLASS.equals(PinpointDrive.class)) {
-            PinpointDrive drive = new PinpointDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-            waitForStart();
-
-            while (opModeIsActive()) {
-                Actions.runBlocking(
-                        drive.actionBuilder(new Pose2d(0, 0, 0))
-                                .lineToX(DISTANCE)
-                                .lineToX(0)
-                                .build());
-            }
-        } else if (TuningOpModes.DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) {
-            SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-            waitForStart();
-
-            while (opModeIsActive()) {
-                Actions.runBlocking(
-                        drive.actionBuilder(new Pose2d(0, 0, 0))
-                                .lineToX(DISTANCE)
-                                .lineToX(0)
-                                .build());
-            }
-        } else if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
-            MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-            if (drive.localizer instanceof TwoDeadWheelLocalizer) {
-                if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) {
-                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
-                }
-            } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) {
-                if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) {
-                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
-                }
-            }
-            waitForStart();
-
-            while (opModeIsActive()) {
-                Actions.runBlocking(
-                        drive.actionBuilder(new Pose2d(0, 0, 0))
-                                .lineToX(DISTANCE)
-                                .lineToX(0)
-                                .build());
-            }
-        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
-            TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-            if (drive.localizer instanceof TwoDeadWheelLocalizer) {
-                if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) {
-                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
-                }
-            } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) {
-                if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) {
-                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
-                }
-            }
-            waitForStart();
-
-            while (opModeIsActive()) {
-                Actions.runBlocking(
-                        drive.actionBuilder(new Pose2d(0, 0, 0))
-                                .lineToX(DISTANCE)
-                                .lineToX(0)
-                                .build());
-            }
-        } else {
-            throw new RuntimeException();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.java
deleted file mode 100644
index 31a67ab..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.java
+++ /dev/null
@@ -1,61 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.tuning;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.ftc.Actions;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.PinpointDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.SparkFunOTOSDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.TankDrive;
-
-public final class SplineTest extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        Pose2d beginPose = new Pose2d(0, 0, 0);
-        if (TuningOpModes.DRIVE_CLASS.equals(PinpointDrive.class)) {
-            PinpointDrive drive = new PinpointDrive(hardwareMap, beginPose);
-
-            waitForStart();
-
-            Actions.runBlocking(
-                    drive.actionBuilder(beginPose)
-                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
-                            .splineTo(new Vector2d(0, 60), Math.PI)
-                            .build());
-        } else if (TuningOpModes.DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) {
-            SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, beginPose);
-
-            waitForStart();
-
-            Actions.runBlocking(
-                    drive.actionBuilder(beginPose)
-                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
-                            .splineTo(new Vector2d(0, 60), Math.PI)
-                            .build());
-        } else if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
-            MecanumDrive drive = new MecanumDrive(hardwareMap, beginPose);
-
-            waitForStart();
-
-            Actions.runBlocking(
-                    drive.actionBuilder(beginPose)
-                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
-                            .splineTo(new Vector2d(0, 60), Math.PI)
-                            .build());
-        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
-            TankDrive drive = new TankDrive(hardwareMap, beginPose);
-
-            waitForStart();
-
-            Actions.runBlocking(
-                    drive.actionBuilder(beginPose)
-                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
-                            .splineTo(new Vector2d(0, 60), Math.PI)
-                            .build());
-        } else {
-            throw new RuntimeException();
-        }
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.java
deleted file mode 100644
index d4abd32..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.java
+++ /dev/null
@@ -1,254 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.tuning;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.reflection.ReflectionConfig;
-import com.acmerobotics.roadrunner.MotorFeedforward;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.ftc.AngularRampLogger;
-import com.acmerobotics.roadrunner.ftc.DeadWheelDirectionDebugger;
-import com.acmerobotics.roadrunner.ftc.DriveType;
-import com.acmerobotics.roadrunner.ftc.DriveView;
-import com.acmerobotics.roadrunner.ftc.DriveViewFactory;
-import com.acmerobotics.roadrunner.ftc.Encoder;
-import com.acmerobotics.roadrunner.ftc.ForwardPushTest;
-import com.acmerobotics.roadrunner.ftc.ForwardRampLogger;
-import com.acmerobotics.roadrunner.ftc.LateralPushTest;
-import com.acmerobotics.roadrunner.ftc.LateralRampLogger;
-import com.acmerobotics.roadrunner.ftc.ManualFeedforwardTuner;
-import com.acmerobotics.roadrunner.ftc.MecanumMotorDirectionDebugger;
-import com.acmerobotics.roadrunner.ftc.OtosEncoder;
-import com.acmerobotics.roadrunner.ftc.PinpointEncoder;
-import com.qualcomm.hardware.lynx.LynxModule;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;
-
-import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta;
-import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.PinpointDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.SparkFunOTOSDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.TankDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer;
-import org.firstinspires.ftc.teamcode.roadrunner.TwoDeadWheelLocalizer;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-public final class TuningOpModes {
-    public static final Class<?> DRIVE_CLASS = PinpointDrive.class; // TODO: change to your drive class i.e. PinpointDrive if using pinpoint
-
-    public static final String GROUP = "quickstart";
-    public static final boolean DISABLED = false;
-
-    private TuningOpModes() {
-    }
-
-    private static OpModeMeta metaForClass(Class<? extends OpMode> cls) {
-        return new OpModeMeta.Builder()
-                .setName(cls.getSimpleName())
-                .setGroup(GROUP)
-                .setFlavor(OpModeMeta.Flavor.TELEOP)
-                .build();
-    }
-
-    @OpModeRegistrar
-    public static void register(OpModeManager manager) {
-        if (DISABLED) return;
-
-        DriveViewFactory dvf;
-        if (DRIVE_CLASS.equals(PinpointDrive.class)) {
-            dvf = hardwareMap -> {
-                PinpointDrive pd = new PinpointDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-                List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
-                List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
-                parEncs.add(new PinpointEncoder(pd.pinpoint, false, pd.leftBack));
-                perpEncs.add(new PinpointEncoder(pd.pinpoint, true, pd.leftBack));
-
-                return new DriveView(
-                        DriveType.MECANUM,
-                        MecanumDrive.PARAMS.inPerTick,
-                        MecanumDrive.PARAMS.maxWheelVel,
-                        MecanumDrive.PARAMS.minProfileAccel,
-                        MecanumDrive.PARAMS.maxProfileAccel,
-                        hardwareMap.getAll(LynxModule.class),
-                        Arrays.asList(
-                                pd.leftFront,
-                                pd.leftBack
-                        ),
-                        Arrays.asList(
-                                pd.rightFront,
-                                pd.rightBack
-                        ),
-                        leftEncs,
-                        rightEncs,
-                        parEncs,
-                        perpEncs,
-                        pd.lazyImu,
-                        pd.voltageSensor,
-                        () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
-                                MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
-                                MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick)
-                );
-            };
-        } else if (DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) {
-            dvf = hardwareMap -> {
-                SparkFunOTOSDrive od = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-                List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
-                List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
-                parEncs.add(new OtosEncoder(od.otos, false, false, od.leftBack));
-                perpEncs.add(new OtosEncoder(od.otos, true, false, od.leftBack));
-
-                return new DriveView(
-                        DriveType.MECANUM,
-                        MecanumDrive.PARAMS.inPerTick,
-                        MecanumDrive.PARAMS.maxWheelVel,
-                        MecanumDrive.PARAMS.minProfileAccel,
-                        MecanumDrive.PARAMS.maxProfileAccel,
-                        hardwareMap.getAll(LynxModule.class),
-                        Arrays.asList(
-                                od.leftFront,
-                                od.leftBack
-                        ),
-                        Arrays.asList(
-                                od.rightFront,
-                                od.rightBack
-                        ),
-                        leftEncs,
-                        rightEncs,
-                        parEncs,
-                        perpEncs,
-                        od.lazyImu,
-                        od.voltageSensor,
-                        () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
-                                MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
-                                MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick)
-                );
-            };
-        } else if (DRIVE_CLASS.equals(MecanumDrive.class)) {
-            dvf = hardwareMap -> {
-                MecanumDrive md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-                List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
-                List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
-                if (md.localizer instanceof MecanumDrive.DriveLocalizer) {
-                    MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer;
-                    leftEncs.add(dl.leftFront);
-                    leftEncs.add(dl.leftBack);
-                    rightEncs.add(dl.rightFront);
-                    rightEncs.add(dl.rightBack);
-                } else if (md.localizer instanceof ThreeDeadWheelLocalizer) {
-                    ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer;
-                    parEncs.add(dl.par0);
-                    parEncs.add(dl.par1);
-                    perpEncs.add(dl.perp);
-                } else if (md.localizer instanceof TwoDeadWheelLocalizer) {
-                    TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) md.localizer;
-                    parEncs.add(dl.par);
-                    perpEncs.add(dl.perp);
-                } else {
-                    throw new RuntimeException("unknown localizer: " + md.localizer.getClass().getName());
-                }
-
-                return new DriveView(
-                        DriveType.MECANUM,
-                        MecanumDrive.PARAMS.inPerTick,
-                        MecanumDrive.PARAMS.maxWheelVel,
-                        MecanumDrive.PARAMS.minProfileAccel,
-                        MecanumDrive.PARAMS.maxProfileAccel,
-                        hardwareMap.getAll(LynxModule.class),
-                        Arrays.asList(
-                                md.leftFront,
-                                md.leftBack
-                        ),
-                        Arrays.asList(
-                                md.rightFront,
-                                md.rightBack
-                        ),
-                        leftEncs,
-                        rightEncs,
-                        parEncs,
-                        perpEncs,
-                        md.lazyImu,
-                        md.voltageSensor,
-                        () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
-                                MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
-                                MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick)
-                );
-            };
-        } else if (DRIVE_CLASS.equals(TankDrive.class)) {
-            dvf = hardwareMap -> {
-                TankDrive td = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
-
-                List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
-                List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
-                if (td.localizer instanceof TankDrive.DriveLocalizer) {
-                    TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) td.localizer;
-                    leftEncs.addAll(dl.leftEncs);
-                    rightEncs.addAll(dl.rightEncs);
-                } else if (td.localizer instanceof ThreeDeadWheelLocalizer) {
-                    ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) td.localizer;
-                    parEncs.add(dl.par0);
-                    parEncs.add(dl.par1);
-                    perpEncs.add(dl.perp);
-                } else if (td.localizer instanceof TwoDeadWheelLocalizer) {
-                    TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) td.localizer;
-                    parEncs.add(dl.par);
-                    perpEncs.add(dl.perp);
-                } else {
-                    throw new RuntimeException("unknown localizer: " + td.localizer.getClass().getName());
-                }
-
-                return new DriveView(
-                        DriveType.TANK,
-                        TankDrive.PARAMS.inPerTick,
-                        TankDrive.PARAMS.maxWheelVel,
-                        TankDrive.PARAMS.minProfileAccel,
-                        TankDrive.PARAMS.maxProfileAccel,
-                        hardwareMap.getAll(LynxModule.class),
-                        td.leftMotors,
-                        td.rightMotors,
-                        leftEncs,
-                        rightEncs,
-                        parEncs,
-                        perpEncs,
-                        td.lazyImu,
-                        td.voltageSensor,
-                        () -> new MotorFeedforward(TankDrive.PARAMS.kS,
-                                TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,
-                                TankDrive.PARAMS.kA / TankDrive.PARAMS.inPerTick)
-                );
-            };
-        } else {
-            throw new RuntimeException();
-        }
-
-        manager.register(metaForClass(AngularRampLogger.class), new AngularRampLogger(dvf));
-        manager.register(metaForClass(ForwardPushTest.class), new ForwardPushTest(dvf));
-        manager.register(metaForClass(ForwardRampLogger.class), new ForwardRampLogger(dvf));
-        manager.register(metaForClass(LateralPushTest.class), new LateralPushTest(dvf));
-        manager.register(metaForClass(LateralRampLogger.class), new LateralRampLogger(dvf));
-        manager.register(metaForClass(ManualFeedforwardTuner.class), new ManualFeedforwardTuner(dvf));
-        manager.register(metaForClass(MecanumMotorDirectionDebugger.class), new MecanumMotorDirectionDebugger(dvf));
-        manager.register(metaForClass(DeadWheelDirectionDebugger.class), new DeadWheelDirectionDebugger(dvf));
-
-        manager.register(metaForClass(ManualFeedbackTuner.class), ManualFeedbackTuner.class);
-        manager.register(metaForClass(SplineTest.class), SplineTest.class);
-        manager.register(metaForClass(LocalizationTest.class), LocalizationTest.class);
-
-        FtcDashboard.getInstance().withConfigRoot(configRoot -> {
-            for (Class<?> c : Arrays.asList(
-                    AngularRampLogger.class,
-                    ForwardRampLogger.class,
-                    LateralRampLogger.class,
-                    ManualFeedforwardTuner.class,
-                    MecanumMotorDirectionDebugger.class,
-                    ManualFeedbackTuner.class
-            )) {
-                configRoot.putVariable(c.getSimpleName(), ReflectionConfig.createVariableFromClass(c));
-            }
-        });
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSAngularScalar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSAngularScalar.java
deleted file mode 100644
index 4dcf578..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSAngularScalar.java
+++ /dev/null
@@ -1,34 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.tuning.otos;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.Rotation2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.roadrunner.SparkFunOTOSDrive;
-
-@TeleOp
-public class OTOSAngularScalar extends LinearOpMode {
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0));
-        double radsTurned = 0;
-        Rotation2d lastHeading = Rotation2d.fromDouble(0);
-        telemetry.addLine("OTOS Angular Scalar Tuner");
-        telemetry.addLine("Press START, then rotate the robot on the ground 10 times (3600 degrees).");
-        telemetry.addLine("Then copy the scalar into SparkFunOTOSDrive.");
-        telemetry.update();
-        waitForStart();
-        while (opModeIsActive()) {
-            drive.updatePoseEstimate();
-            radsTurned += drive.pose.heading.minus(lastHeading);
-            lastHeading = drive.pose.heading;
-            telemetry.addData("Uncorrected Degrees Turned", Math.toDegrees(radsTurned));
-            telemetry.addData("Calculated Angular Scalar", 3600 / Math.toDegrees(radsTurned));
-            telemetry.update();
-        }
-
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSHeadingOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSHeadingOffsetTuner.java
deleted file mode 100644
index f3f4b29..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSHeadingOffsetTuner.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.tuning.otos;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.roadrunner.SparkFunOTOSDrive;
-
-@TeleOp
-public class OTOSHeadingOffsetTuner extends LinearOpMode {
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0));
-        telemetry.addLine("OTOS Heading Offset Tuner");
-        telemetry.addLine("Line the side of the robot against a wall and Press START.");
-        telemetry.addLine("Then push the robot forward some distance.");
-        telemetry.addLine("Finally, copy the heading offset into line 38 of SparkFunOTOSDrive");
-        telemetry.update();
-        waitForStart();
-        while (opModeIsActive()) {
-            drive.updatePoseEstimate();
-            telemetry.addData("Heading Offset (radians, enter this one into SparkFunOTOSDrive!)", Math.atan2(drive.pose.position.y, drive.pose.position.x));
-            telemetry.addData("Heading Offset (degrees)", Math.toDegrees(Math.atan2(drive.pose.position.y, drive.pose.position.x)));
-            telemetry.update();
-        }
-
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSPositionOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSPositionOffsetTuner.java
deleted file mode 100644
index 0f54b41..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/otos/OTOSPositionOffsetTuner.java
+++ /dev/null
@@ -1,35 +0,0 @@
-package org.firstinspires.ftc.teamcode.roadrunner.tuning.otos;
-
-import com.acmerobotics.roadrunner.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.roadrunner.SparkFunOTOSDrive;
-
-@TeleOp
-public class OTOSPositionOffsetTuner extends LinearOpMode {
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0));
-        telemetry.addLine("OTOS Position Offset Tuner");
-        telemetry.addLine("Line the robot against the corner of two walls facing forward and Press START.");
-        telemetry.addLine("Then rotate the robot exactly 180 degrees and press it back into the corner.");
-        telemetry.addLine("Finally, copy the pose offset into line 38 of SparkFunOTOSDrive.");
-        telemetry.update();
-        waitForStart();
-        while (opModeIsActive()) {
-            drive.updatePoseEstimate();
-            telemetry.addData("Heading (deg)", Math.toDegrees(drive.pose.heading.toDouble()));
-            if (Math.abs(Math.toDegrees(drive.pose.heading.toDouble())) > 175) {
-                telemetry.addData("X Offset", drive.pose.position.x / 2);
-                telemetry.addData("Y Offset", drive.pose.position.y / 2);
-            } else {
-                telemetry.addLine("Rotate the robot 180 degrees and align it to the corner again.");
-            }
-            telemetry.update();
-        }
-
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Arm.java
new file mode 100644
index 0000000..610a279
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Arm.java
@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Config
+public class Arm extends SubsystemBase {
+    public static double ACCEPTING = 0.3;
+    public static double SCORE = 0.95;
+    public static double READY = 0.9;
+    public static double INTAKE = 0.15;
+    public static double SPECIMEN = 0.6;
+    public static double DRIVE_IN = 0.35;
+    public static double SWEEP = 0;
+
+    private final Servo left, right;
+
+    public Arm(final HardwareMap hwMap) {
+        this.left = hwMap.get(Servo.class, "leftArm");
+        this.right = hwMap.get(Servo.class, "rightArm");
+
+        this.setPosition(Arm.READY);
+    }
+
+    public void setPosition(double position) {
+        this.left.setPosition(position);
+        this.right.setPosition(1 - position);
+    }
+
+    public double getPosition() {
+        return this.left.getPosition();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java
index 897325b..f43e5e3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java
@@ -1,52 +1,28 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.Servo;
 
 @Config
 public class Claw extends SubsystemBase {
-    public static double SCORE = 1;
-    public static double ACCEPTING = 0.55;
+    public static double OPEN = 0;
+    public static double CLOSE = 0.3;
 
-    //TODO: Get positions for claw rotate
-    public static double UP = 1;
-    public static double DOWN = 0.49;
-    public static double READY = 0;
+    private final Servo claw;
 
-    private final MultipleTelemetry telemetry;
+    public Claw(final HardwareMap hwMap) {
+        this.claw = hwMap.get(Servo.class, "claw");
 
-    private final Servo leftPivot, rightPivot, clawRotate;
-    private final CRServo leftClaw, rightClaw;
-
-
-    public Claw(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
-        this.telemetry = telemetry;
-
-        this.leftPivot = hwMap.get(Servo.class, "leftPivot");
-        this.rightPivot = hwMap.get(Servo.class, "rightPivot");
-        this.leftClaw = hwMap.get(CRServo.class, "leftClaw");
-        this.rightClaw = hwMap.get(CRServo.class, "rightClaw");
-        this.clawRotate = hwMap.get(Servo.class, "clawRotate");
-
-        this.setPivotPosition(Claw.SCORE);
-        this.setRotatePosition(Claw.READY);
-    }
-
-    public void setPivotPosition(double position) {
-        this.leftPivot.setPosition(position);
-        this.rightPivot.setPosition(1 - position);
+        this.setPosition(Claw.CLOSE);
     }
 
-    public void setRotatePosition(double position) {
-        this.clawRotate.setPosition(position);
+    public double getPosition() {
+        return this.claw.getPosition();
     }
 
-    public void setClawPower(double power) {
-        this.leftClaw.setPower(power);
-        this.rightClaw.setPower(-power);
+    public void setPosition(double position) {
+        this.claw.setPosition(position);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
index 62502df..7ddff42 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
@@ -1,21 +1,21 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.acmerobotics.roadrunner.Vector2d;
 import com.arcrobotics.ftclib.command.CommandOpMode;
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.localization.Pose;
+import com.pedropathing.util.Constants;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.FConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.constants.LConstants;
 
 @Config
 public class Drivetrain extends SubsystemBase {
     public static double MAX_ACCEL = 0.3;
-    public static double MAX_ANGULAR_ACCEL = 0.1;
+    public static double MAX_ANGULAR_ACCEL = 0.2;
 
     public static double MAX_DEACCEL = 0.5;
     public static double MAX_ANGULAR_DEACCEL = 0.5;
@@ -23,17 +23,16 @@ public class Drivetrain extends SubsystemBase {
     public static double MAX_VEL = 0.75;
     public static double MAX_ANGULAR_VEL = 0.6;
 
-    private final MecanumDrive drive;
-    private final MultipleTelemetry telemetry;
+    private final Follower follower;
 
     private double xPower = 0.0;
     private double angPower = 0.0;
     private double yPower = 0.0;
 
-    public Drivetrain(final HardwareMap hwMap, final MultipleTelemetry telemetry, Pose2d startPose) {
-        this.telemetry = telemetry;
-
-        this.drive = new MecanumDrive(hwMap, startPose);
+    public Drivetrain(final HardwareMap hwMap, Pose startPose) {
+        Constants.setConstants(FConstants.class, LConstants.class);
+        this.follower = new Follower(hwMap);
+        this.follower.setStartingPose(startPose);
     }
 
     private static double calculateAccel(double accel, double deaccel, double prevPower, double check) {
@@ -48,19 +47,17 @@ private static double calculateAccel(double accel, double deaccel, double prevPo
 
     public void startThread(final GamepadEx gamepad, CommandOpMode opMode) {
         new Thread(() -> {
+            this.follower.startTeleopDrive();
+
             while (opMode.opModeIsActive())
                 try {
-                    synchronized (this.drive) {
+                    synchronized (this.follower) {
                         this.yPower += Drivetrain.calculateAccel(MAX_ACCEL, MAX_DEACCEL, this.yPower, gamepad.getLeftY());
-                        this.xPower += Drivetrain.calculateAccel(MAX_ACCEL, MAX_DEACCEL, this.xPower, gamepad.getLeftX());
+                        this.xPower += Drivetrain.calculateAccel(MAX_ACCEL, MAX_DEACCEL, this.xPower, -gamepad.getLeftX());
                         this.angPower += Drivetrain.calculateAccel(MAX_ANGULAR_ACCEL, MAX_ANGULAR_DEACCEL, this.angPower, gamepad.getRightX());
 
-                        this.drive.setDrivePowers(new PoseVelocity2d(
-                                new Vector2d(
-                                        this.yPower * MAX_VEL,
-                                        -this.xPower * MAX_VEL
-                                ),
-                                -this.angPower * MAX_ANGULAR_VEL));
+                        this.follower.setTeleOpMovementVectors(yPower * MAX_VEL, xPower * MAX_VEL, angPower * MAX_ANGULAR_VEL, false);
+                        this.follower.update();
                     }
                     Thread.sleep(10);
                 } catch (InterruptedException e) {
@@ -69,7 +66,7 @@ public void startThread(final GamepadEx gamepad, CommandOpMode opMode) {
         }).start();
     }
 
-    public MecanumDrive getDrive() {
-        return drive;
+    public Follower getFollower() {
+        return this.follower;
     }
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Extendo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Extendo.java
index 221a894..cc520b1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Extendo.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Extendo.java
@@ -1,51 +1,89 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.command.CommandOpMode;
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.qualcomm.robotcore.hardware.AnalogInput;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.teamcode.utils.PIDController;
+import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
 @Config
 public class Extendo extends SubsystemBase {
-    public static double kP = 0.015;
-    public static double kI = 0.0001;
-    public static double kD = 0.0001;
-    public static double OFFSET = 306.0;
+    public static double kP = 0.005;
+    public static double kI = 0;
+    public static double kD = 0;
 
-    public static int ALLOWED_ERROR = 15;
-    public static int SCORE = 0;
-    public static int READY = 100;
-    public static int ACCEPTING = 350;
+    public static double OFFSET = 11.45;
+    public static int ALLOWED_ERROR = 3;
+    public static int SCORE = -30;
+    public static int READY = 70;
+    public static int ACCEPTING = 250;
+    public static int SPECIMEN = 100;
+    public static int DRIVE_IN = 120;
+    public static double SWEEP = 0;
+    public static int TELE_OFFSET = 75;
 
-    private final MultipleTelemetry telemetry;
+    private double power;
+    private boolean toggleController;
 
     public final AnalogInput encoder;
-    private final CRServo extendo;
+    private final CRServo extendoBottom;
+    private final CRServo extendoTop;
     private final PIDController controller;
 
-    public Extendo(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
-        this.telemetry = telemetry;
+    private double rotations = 0;
+    private double prevPos = 0;
+
+    public Extendo(final HardwareMap hwMap) {
         this.controller = new PIDController(Extendo.kP, Extendo.kI, Extendo.kD);
 
-        this.extendo = hwMap.get(CRServo.class, "extendo");
+        this.extendoBottom = hwMap.get(CRServo.class, "extendoBottom");
+        this.extendoTop = hwMap.get(CRServo.class, "extendoTop");
         this.encoder = hwMap.get(AnalogInput.class, "encoder");
+
+        this.controller.setAllowedError(Extendo.ALLOWED_ERROR);
     }
 
     public synchronized double getPosition() {
-        return Extendo.OFFSET - (this.encoder.getVoltage() / 3.3 * 360);
+        return -(Extendo.OFFSET - (this.encoder.getVoltage() / 3.3 * 360));
     }
 
-    public void startThread(CommandOpMode opMode) {
-        while (opMode.opModeIsActive())
-            synchronized (this.extendo) {
-                double power = this.controller.calculate(this.getPosition());
-                this.extendo.setPower(power);
+    public void startThread(OpModeCore opMode) {
+        new Thread(() -> {
+            while (opMode.opModeIsActive()) {
+                try {
+                    double currPos = this.getPosition();
+
+                    if (prevPos - currPos > 180) rotations++;
+                    else if (180 < currPos - prevPos) rotations--;
+
+                    if (this.toggleController) {
+
+
+                        this.power = this.controller.calculate(currPos + 360 * rotations);
+                        if (!this.controller.isFinished()) {
+                            synchronized (this.extendoBottom) {
+                                this.extendoBottom.setPower(-this.power);
+                            }
+
+                            synchronized (this.extendoTop) {
+                                this.extendoTop.setPower(-this.power);
+                            }
+                        }
+                    } else {
+                        this.extendoTop.setPower(this.power);
+                        this.extendoBottom.setPower(this.power);
+                    }
+
+                    prevPos = currPos;
+                    Thread.sleep(50);
+                } catch (Exception e) {
+                    e.printStackTrace();
+                }
             }
+        }).start();
     }
 
     public void setConstants() {
@@ -57,9 +95,15 @@ public double getTarget() {
     }
 
     public void setPosition(double position) {
+        this.toggleController = true;
         this.controller.setTarget(position);
     }
 
+    public void setPower(double power) {
+        this.toggleController = false;
+        this.power = power;
+    }
+
     public double getError() {
         return this.controller.getLastError();
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
index b1e94da..1865df5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.CommandOpMode;
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -14,56 +13,74 @@
 
 @Config
 public class Lift extends SubsystemBase {
-    public static double MIN_POWER = -0.25;
-    public static double LOW_VOLTAGE = 12.0;
-    public static double ALLOWED_ERROR = 15;
+    public static double MIN_POWER = -0.1;
+
+    //placeholder lift values
+    public static double LOW_BASKET = 400;
+    public static double LOW_RUNG = 400;
+    public static double HIGH_RUNG = 500;
+    public static double DRIVE_IN = 465;
+    public static double CLIMB = -Integer.MAX_VALUE;
+
+    public static double ACCEPTING = 10;
+    public static double INCREMENT = 50;
+
+    public static double SLAM = 250;
+
+    public static double lowkP = 0.009;
+    public static double lowkI = 0;
+    public static double lowkD = 0;
+    public static double lowkG = 0;
+
+    public static double highkP = 0.007;
+    public static double highkI = 0;
+    public static double highkD = 0;
+    public static double highkG = 0;
+
+    public static double LOW_VOLTAGE = 12;
+
+    private final DcMotorEx rightSec;
+    private final DcMotorEx leftSec;
+    private final DcMotorEx rightPri;
+    private final DcMotorEx leftPri;
+    private final VoltageSensor voltageSensor;
+    private final PIDController highController;
+    private final PIDController lowController;
+
+    public Lift(final HardwareMap hwMap) {
+        this.rightSec = hwMap.get(DcMotorEx.class, "rightLiftSec");
+        this.leftSec = hwMap.get(DcMotorEx.class, "leftLiftSec");
+        this.rightPri = hwMap.get(DcMotorEx.class, "rightLiftPri");
+        this.leftPri = hwMap.get(DcMotorEx.class, "leftLiftPri");
+
+        this.leftPri.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        this.rightSec.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        this.rightPri.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        this.leftSec.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        this.leftSec.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        this.rightSec.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        this.rightPri.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        this.leftPri.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        this.leftSec.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.rightSec.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.rightPri.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.leftPri.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        this.leftSec.setDirection(DcMotorSimple.Direction.REVERSE);
+        this.rightSec.setDirection(DcMotorSimple.Direction.FORWARD);
+        this.rightPri.setDirection(DcMotorSimple.Direction.REVERSE);
+        this.leftPri.setDirection(DcMotorSimple.Direction.FORWARD);
+
+        this.highController = new PIDController(Lift.highkP, Lift.highkI, Lift.highkD, Lift.highkG);
+        this.highController.setAllowedError(15);
+        this.lowController = new PIDController(Lift.lowkP, Lift.lowkI, Lift.lowkD, Lift.lowkG);
+        this.lowController.setAllowedError(15);
+
+        this.voltageSensor = hwMap.get(VoltageSensor.class, "Control Hub");
 
-    public static double LOW_BASKET = 1500;
-    public static double HIGH_BASKET = 2800;
-
-    public static double LOW_RUNG = 1000;
-    public static double HIGH_RUNG = 2100;
-
-    public static double ACCEPTING = 50;
-    public static double INCREMENT = 250;
-    public static double SLAM = 750;
-
-    public static double kP = 0.0015;
-    public static double kI = 0.0006;
-    public static double kD = 0.000015;
-    public static double kG = 0.063;
-
-    private final DcMotorEx rightMotor;
-    private final DcMotorEx leftMotor;
-    private final VoltageSensor voltage;
-    private final PIDController controller;
-
-    private final MultipleTelemetry telemetry;
-
-    public Lift(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
-        this.telemetry = telemetry;
-        this.voltage = hwMap.get(VoltageSensor.class, "Control Hub");
-
-        this.rightMotor = hwMap.get(DcMotorEx.class, "liftRight");
-        this.leftMotor = hwMap.get(DcMotorEx.class, "liftLeft");
-
-        this.leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        this.rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        this.leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        this.rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        this.leftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        this.rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
-        this.leftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-        this.rightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
-
-        this.controller = new PIDController(kP, kI, kD);
-        this.controller.setAllowedError(Lift.ALLOWED_ERROR);
-
-        // TODO: Make this work.
-        // this.reset();
+        this.setTarget(0);
     }
 
     public void startThread(CommandOpMode opMode) {
@@ -71,22 +88,28 @@ public void startThread(CommandOpMode opMode) {
             while (opMode.opModeIsActive())
                 try {
                     double power;
+                    double voltage;
+                    voltage = voltageSensor.getVoltage();
+
+                    synchronized (this.rightPri) {
+                        if (voltage > LOW_VOLTAGE) {
+                            power = this.lowController.calculate(this.getPosition());
+                        } else {
+                            power = this.highController.calculate(this.getPosition());
+                        }
+                        this.rightPri.setPower(Math.max(power, Lift.MIN_POWER));
+                    }
 
-                    synchronized (this.rightMotor) {
-                        power = this.controller.calculate(this.rightMotor.getCurrentPosition());
+                    synchronized (this.rightSec) {
+                        this.rightSec.setPower(Math.max(power, Lift.MIN_POWER));
                     }
 
-                    if (!this.controller.isFinished()) {
-                        synchronized (this.rightMotor) {
-                            this.rightMotor.setPower(Math.max(power, Lift.MIN_POWER));
-                        }
+                    synchronized (this.leftPri) {
+                        this.leftPri.setPower(Math.max(power, Lift.MIN_POWER));
+                    }
 
-                        synchronized (this.leftMotor) {
-                            this.leftMotor.setPower(Math.max(power, Lift.MIN_POWER));
-                        }
-                    } else {
-                        this.leftMotor.setPower(Lift.kG);
-                        this.rightMotor.setPower(Lift.kG);
+                    synchronized (this.leftSec) {
+                        this.leftSec.setPower(Math.max(power, Lift.MIN_POWER));
                     }
 
                     Thread.sleep(50);
@@ -96,47 +119,42 @@ public void startThread(CommandOpMode opMode) {
         }).start();
     }
 
-    public void setPower(double power) {
-        this.leftMotor.setPower(power);
-        this.rightMotor.setPower(power);
-    }
-
     public double getTarget() {
-        return this.controller.getTarget();
+        return this.lowController.getTarget();
     }
 
     public void setTarget(double target) {
-        this.controller.setTarget(target);
+        this.highController.setTarget(target);
+        this.lowController.setTarget(target);
     }
 
     public double getPosition() {
-        return this.rightMotor.getCurrentPosition();
+        return -this.leftPri.getCurrentPosition();
     }
 
-    public double getVoltage() {
-        return this.voltage.getVoltage();
+    public void setPower(double power) {
+        this.rightSec.setPower(power);
+        this.rightPri.setPower(power);
+        this.leftPri.setPower(power);
+        this.leftSec.setPower(power);
     }
 
     public boolean isFinished() {
-        return this.controller.isFinished();
+        double voltage;
+        voltage = voltageSensor.getVoltage();
+        if (voltage < LOW_VOLTAGE) {
+            return this.lowController.isFinished();
+        } else {
+            return this.highController.isFinished();
+        }
     }
 
     public double getError() {
-        return this.controller.getLastError();
+        return this.lowController.getLastError();
     }
 
     public void setConstants() {
-        this.controller.setCoefficients(kP, kI, kD);
-    }
-
-    public void reset() {
-        this.setTarget(-999999999);
-        while (this.voltage.getVoltage() > Lift.LOW_VOLTAGE) {
-        }
-
-        this.leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        this.rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
-        this.setTarget(0);
+        this.lowController.setCoefficients(Lift.lowkP, Lift.lowkI, Lift.lowkD, Lift.lowkG);
+        this.highController.setCoefficients(Lift.highkP, Lift.highkI, Lift.highkD, Lift.highkG);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Pivot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Pivot.java
new file mode 100644
index 0000000..1352d6b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Pivot.java
@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Config
+public class Pivot extends SubsystemBase {
+    public static double ACCEPTING = 0;
+    public static double READY = 0;
+    public static double SCORE = 0.8;
+    public static double SPECIMEN = 0.05;
+    public static double INTAKE = 0.1;
+    public static double DRIVE_IN = 0.8;
+    public static double SWEEP = 0;
+
+    private final Servo pivot;
+
+    public Pivot(final HardwareMap hwMap) {
+        this.pivot = hwMap.get(Servo.class, "pivot");
+
+        this.setPosition(Pivot.READY);
+    }
+
+    public void setPosition(double position) {
+        this.pivot.setPosition(position);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Wrist.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Wrist.java
new file mode 100644
index 0000000..d7ab768
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Wrist.java
@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Config
+public class Wrist extends SubsystemBase {
+    public static double SCORE = 0;
+    public static double ACCEPTING = 0;
+    public static double READY = 0;
+    public static double SPECIMEN = 1;
+
+    public static double INCREMENT = 0.25;
+
+    private final Servo wrist;
+
+    public Wrist(final HardwareMap hwMap) {
+        this.wrist = hwMap.get(Servo.class, "wrist");
+
+        this.setPosition(Wrist.ACCEPTING);
+    }
+
+    public void setPosition(double position) {
+        this.wrist.setPosition(position);
+    }
+
+    public double getPosition() {
+        return this.wrist.getPosition();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousHelpers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousHelpers.java
new file mode 100644
index 0000000..35ac517
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousHelpers.java
@@ -0,0 +1,131 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.pedropathing.localization.Pose;
+import com.pedropathing.pathgen.BezierCurve;
+import com.pedropathing.pathgen.BezierLine;
+import com.pedropathing.pathgen.Path;
+import com.pedropathing.pathgen.Point;
+
+import org.json.JSONArray;
+import org.json.JSONException;
+import org.json.JSONObject;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.Scanner;
+
+public class AutonomousHelpers {
+
+    public enum HeadingInterpolation {
+        LINEAR,
+        CONSTANT,
+        TANGENT
+    }
+
+    public static Path buildLine(Pose startPose, Pose endPose, HeadingInterpolation interpolation) {
+        Point startPoint = new Point(startPose.getX(), startPose.getY());
+        Point endPoint = new Point(endPose.getX(), endPose.getY());
+
+        Path path = new Path(new BezierLine(startPoint, endPoint));
+        setHeadingInterpolation(path, startPose.getHeading(), endPose.getHeading(), interpolation);
+
+        return path;
+    }
+
+    public static Path buildCurve(Pose startPose, Point controlPoint, Pose endPose,
+                                  HeadingInterpolation interpolation) {
+        Point startPoint = new Point(startPose.getX(), startPose.getY());
+        Point endPoint = new Point(endPose.getX(), endPose.getY());
+
+        Path path = new Path(new BezierCurve(startPoint, controlPoint, endPoint));
+        setHeadingInterpolation(path, startPose.getHeading(), endPose.getHeading(), interpolation);
+
+        return path;
+    }
+
+    public static Path buildCurve(Pose startPose, Point firstControlPoint, Point secondControlPoint,
+                                  Pose endPose, HeadingInterpolation interpolation) {
+        Point startPoint = new Point(startPose.getX(), startPose.getY());
+        Point endPoint = new Point(endPose.getX(), endPose.getY());
+
+        Path path = new Path(new BezierCurve(startPoint, firstControlPoint, secondControlPoint, endPoint));
+        setHeadingInterpolation(path, startPose.getHeading(), endPose.getHeading(), interpolation);
+
+        return path;
+    }
+
+    public static Path buildCurve(Pose startPose, Point firstControlPoint, Point secondControlPoint,
+                                  Point thirdControlPoint, Pose endPose, HeadingInterpolation interpolation) {
+        Point startPoint = new Point(startPose.getX(), startPose.getY());
+        Point endPoint = new Point(endPose.getX(), endPose.getY());
+
+        Path path = new Path(new BezierCurve(startPoint, firstControlPoint, secondControlPoint, thirdControlPoint, endPoint));
+        setHeadingInterpolation(path, startPose.getHeading(), endPose.getHeading(), interpolation);
+
+        return path;
+    }
+
+    public static ArrayList<Pose> getPoses(String path) {
+        try {
+            ArrayList<Pose> poses = new ArrayList<Pose>();
+            String jsonString = "";
+
+            File file = new File(path);
+            Scanner reader = new Scanner(file);
+
+            while (reader.hasNextLine())
+                jsonString += reader.nextLine();
+
+            JSONObject data = new JSONObject(jsonString);
+            JSONArray lines = data.getJSONArray("lines");
+
+            poses.add(new Pose(
+                    data.getJSONObject("startPoint").getDouble("x"),
+                    data.getJSONObject("startPoint").getDouble("y"),
+                    Math.toRadians(lines.getJSONObject(0).getJSONObject("endPoint").getDouble("startDeg"))
+            ));
+
+            for (int i = 0; i < lines.length(); i++) {
+                JSONObject line = lines.getJSONObject(i);
+                System.out.println(line);
+                poses.add(getPose(line.getJSONObject("endPoint")));
+
+                JSONArray controlPoints = line.getJSONArray("controlPoints");
+                for (int j = 0; j < controlPoints.length(); j++) {
+                    poses.add(getPose(controlPoints.getJSONObject(j)));
+                }
+            }
+
+            return poses;
+        } catch (Exception e) {
+            return null;
+        }
+    }
+
+    private static Pose getPose(JSONObject object) throws JSONException {
+        return new Pose(
+                object.getDouble("x"),
+                object.getDouble("y"),
+                object.has("startDeg") ? Math.toRadians(object.getDouble("startDeg")) : -1.0
+        );
+    }
+
+    public static Point poseToPoint(Pose pose) {
+        return new Point(pose.getX(), pose.getY());
+    }
+
+    private static void setHeadingInterpolation(Path path, double startHeading, double endHeading,
+                                                HeadingInterpolation interpolation) {
+        switch (interpolation) {
+            case LINEAR:
+                path.setLinearHeadingInterpolation(startHeading, endHeading);
+                break;
+            case CONSTANT:
+                path.setConstantHeadingInterpolation(startHeading);
+                break;
+            case TANGENT:
+                path.setTangentHeadingInterpolation();
+                break;
+        }
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/TelemetryCore.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/TelemetryCore.java
new file mode 100644
index 0000000..4aad2d9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/TelemetryCore.java
@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+
+public class TelemetryCore {
+    private static MultipleTelemetry instance;
+
+    public TelemetryCore(MultipleTelemetry telemetry) {
+        TelemetryCore.instance = telemetry;
+    }
+
+    public static synchronized MultipleTelemetry getInstance() {
+        return TelemetryCore.instance;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/CommandAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/CommandAction.java
deleted file mode 100644
index c649690..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/CommandAction.java
+++ /dev/null
@@ -1,34 +0,0 @@
-package org.firstinspires.ftc.teamcode.utils.commands;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-import com.arcrobotics.ftclib.command.Command;
-import com.arcrobotics.ftclib.command.CommandScheduler;
-
-public class CommandAction implements Action {
-    private final Command command;
-    private boolean initialized = false;
-    private boolean finished = false;
-
-    public CommandAction(Command command) {
-        this.command = command;
-    }
-
-    @Override
-    public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-        if (!initialized) {
-            command.initialize();
-            initialized = true;
-        } else if (command.isFinished()) {
-            command.end(false);
-            CommandScheduler.getInstance().run();
-            finished = true;
-        } else {
-            CommandScheduler.getInstance().run();
-            command.execute();
-        }
-        return !finished;
-    }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/OpModeCore.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/OpModeCore.java
index cba7770..d68dbd2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/OpModeCore.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/OpModeCore.java
@@ -6,14 +6,21 @@
 import com.arcrobotics.ftclib.command.CommandScheduler;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
+import org.firstinspires.ftc.teamcode.utils.TelemetryCore;
+
 public abstract class OpModeCore extends CommandOpMode {
-    public final MultipleTelemetry multipleTelemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
     private final ElapsedTime period = new ElapsedTime();
-    private final ElapsedTime startUp = new ElapsedTime();
 
     public void logCycles() {
-        this.multipleTelemetry.addData("Period (Seconds / 1 Cycle): ", this.period.seconds());
-        this.multipleTelemetry.addData("Frequency (Hz, Cycles / 1 Second ): ", 1 / this.period.seconds());
+        TelemetryCore.getInstance().addData("Period (Seconds / 1 Cycle): ", this.period.seconds());
+        TelemetryCore.getInstance().addData("Frequency (Hz, Cycles / 1 Second ): ", 1 / this.period.seconds());
+    }
+
+    @Override
+    public void initialize() {
+        new TelemetryCore(new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()));
+        CommandScheduler.getInstance().enable();
+        CommandScheduler.getInstance().reset();
     }
 
     public void end() {
@@ -22,14 +29,6 @@ public void end() {
         CommandScheduler.getInstance().reset();
     }
 
-    public void resetStartUp() {
-        this.startUp.reset();
-    }
-
-    public void logStartUp() {
-        this.multipleTelemetry.addData("Start Up Time (Seconds): ", this.startUp.seconds());
-    }
-
     public void resetCycle() {
         this.period.reset();
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathChainCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathChainCommand.java
new file mode 100644
index 0000000..a9098c3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathChainCommand.java
@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.utils.commands;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.pathgen.Path;
+import com.pedropathing.pathgen.PathChain;
+
+import org.firstinspires.ftc.teamcode.CommandRobot;
+
+public class PathChainCommand extends CommandBase {
+    private final PathChain pathChain;
+    private final double speed;
+    private final Follower follower;
+
+    public PathChainCommand(CommandRobot robot, Path... paths) {
+        this.pathChain = new PathChain(paths);
+        this.speed = 1;
+        this.follower = robot.getFollower();
+    }
+
+    public PathChainCommand(CommandRobot robot, double speed, Path... paths) {
+        this.pathChain = new PathChain(paths);
+        this.follower = robot.getFollower();
+        this.speed = speed;
+    }
+
+    @Override
+    public void initialize() {
+        follower.setMaxPower(speed);
+        follower.followPath(pathChain, true);
+    }
+
+    @Override
+    public void execute() {
+        follower.update();
+        follower.getDashboardPoseTracker().update();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Thread.currentThread().isInterrupted() || !follower.isBusy();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        follower.setMaxPower(1);
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathCommand.java
new file mode 100644
index 0000000..a4f3021
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/PathCommand.java
@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.utils.commands;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.pathgen.Path;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.CommandRobot;
+
+public class PathCommand extends CommandBase {
+    private final Path path;
+    private final Follower follower;
+    private final double speed;
+    private final ElapsedTime timer;
+
+    public PathCommand(CommandRobot robot, Path path) {
+        this.path = path;
+        this.speed = 1;
+        this.timer = new ElapsedTime();
+        this.follower = robot.getFollower();
+    }
+
+    public PathCommand(CommandRobot robot, Path path, double speed) {
+        this.path = path;
+        this.timer = new ElapsedTime();
+        this.follower = robot.getFollower();
+        this.speed = speed;
+    }
+
+    @Override
+    public void initialize() {
+        this.timer.reset();
+        this.follower.setMaxPower(speed);
+        this.follower.followPath(path, true);
+    }
+
+    @Override
+    public void execute() {
+        this.follower.update();
+        this.follower.getDashboardPoseTracker().update();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Thread.currentThread().isInterrupted() || !this.follower.isBusy() || this.timer.seconds() > path.getPathEndTimeoutConstraint();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        this.follower.setMaxPower(1);
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/WaitUntilPosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/WaitUntilPosition.java
deleted file mode 100644
index 4461aad..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/commands/WaitUntilPosition.java
+++ /dev/null
@@ -1,21 +0,0 @@
-package org.firstinspires.ftc.teamcode.utils.commands;
-
-import com.acmerobotics.roadrunner.Pose2d;
-
-import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
-
-public class WaitUntilPosition {
-    private final Pose2d target;
-    private final Drivetrain drivetrain;
-    private final double tolerance;
-
-    public WaitUntilPosition(final Pose2d target, final Drivetrain drivetrain, final double tolerance) {
-        this.target = target;
-        this.drivetrain = drivetrain;
-        this.tolerance = tolerance;
-    }
-
-    public boolean isFinished() {
-        return Math.hypot(this.drivetrain.getDrive().pose.position.x - this.target.position.x, this.drivetrain.getDrive().pose.position.y - this.target.position.y) < this.tolerance && Math.abs(this.drivetrain.getDrive().pose.heading.toDouble() - this.target.heading.toDouble()) < this.tolerance;
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedIMU.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedIMU.java
index 1244f2a..d7ce822 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedIMU.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/hardware/EnhancedIMU.java
@@ -1,6 +1,5 @@
 package org.firstinspires.ftc.teamcode.utils.hardware;
 
-import com.acmerobotics.roadrunner.ftc.LazyImu;
 import com.arcrobotics.ftclib.command.CommandOpMode;
 import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
@@ -18,10 +17,6 @@ public EnhancedIMU(IMU imu, ImuOrientationOnRobot orientation) {
         this.imu.resetYaw();
     }
 
-    public EnhancedIMU(LazyImu imu) {
-        this.imu = imu.get();
-    }
-
     public void startThread(CommandOpMode opMode) {
         new Thread(() -> {
             while (opMode.opModeIsActive()) {
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
index 593b31a..5266818 100644
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -1,17 +1,25 @@
 repositories {
     mavenCentral()
+    maven { url = 'https://maven.brott.dev/' }
+    maven { url = 'https://maven.pedropathing.com/' }
     google() // Needed for androidx
 }
 
 dependencies {
-    implementation 'org.firstinspires.ftc:Inspection:10.1.1'
-    implementation 'org.firstinspires.ftc:Blocks:10.1.1'
-    implementation 'org.firstinspires.ftc:RobotCore:10.1.1'
-    implementation 'org.firstinspires.ftc:RobotServer:10.1.1'
-    implementation 'org.firstinspires.ftc:OnBotJava:10.1.1'
-    implementation 'org.firstinspires.ftc:Hardware:10.1.1'
-    implementation 'org.firstinspires.ftc:FtcCommon:10.1.1'
-    implementation 'org.firstinspires.ftc:Vision:10.1.1'
+    implementation 'org.firstinspires.ftc:Inspection:10.2.0'
+    implementation 'org.firstinspires.ftc:Blocks:10.2.0'
+    implementation 'org.firstinspires.ftc:RobotCore:10.2.0'
+    implementation 'org.firstinspires.ftc:RobotServer:10.2.0'
+    implementation 'org.firstinspires.ftc:OnBotJava:10.2.0'
+    implementation 'org.firstinspires.ftc:Hardware:10.2.0'
+    implementation 'org.firstinspires.ftc:FtcCommon:10.2.0'
+    implementation 'org.firstinspires.ftc:Vision:10.2.0'
+
     implementation 'androidx.appcompat:appcompat:1.2.0'
+
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.16'
+
+    implementation 'com.pedropathing:pedro:1.0.8'
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.16'
 }
 
diff --git a/build.gradle b/build.gradle
index 435bad2..09e8b49 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,3 +1,4 @@
+
 /**
  * Top-level build file for ftc_app project.
  *
diff --git a/pipelines/apriltag.vpr b/pipelines/apriltag.vpr
deleted file mode 100644
index 82b620d..0000000
--- a/pipelines/apriltag.vpr
+++ /dev/null
@@ -1 +0,0 @@
-{"area_max":100.0,"area_min":0.001,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":500.0,"botfloorsnap":1,"botlength":0.4064,"bottype":"swerve","botwidth":0.38597501999914,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"coral","contour_grouping":0,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":10.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"desc":"Apriltag","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"coral","dilation_steps":0,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":0,"exposure":800.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"","fiducial_locfilters":"","fiducial_qualitythreshold":0.0,"fiducial_resdiv":4,"fiducial_size":165.1,"fiducial_skip3d":0,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dbotposefieldspace","flicker":0,"force_convex":1,"hue_max":85,"hue_min":55,"image_flip":0,"image_source":0,"img_to_show":0,"intersection_filter":0,"invert_hue":0,"lcgain":15.0,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":1,"pipeline_led_power":100,"pipeline_res":3,"pipeline_type":"pipe_fiducial","python_snapscript_name":"","quality_focus":0.3,"red_balance":2500.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.0,"rspitch":0.0,"rsroll":0.0,"rss":0.0,"rsu":0.0,"rsyaw":0.0,"sat_max":255,"sat_min":70,"send_corners":0,"send_json":0,"tsf":0.0,"tss":0.0,"tsu":0.0,"val_max":255,"val_min":70,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0}
\ No newline at end of file
diff --git a/settings.gradle b/settings.gradle
index 8dc221c..9e2cfb3 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -1,3 +1,2 @@
 include ':FtcRobotController'
 include ':TeamCode'
-include ':PathVisualizer'