Try to fix the camera

FtcRobotController/src/main/AndroidManifest.xml

@@ -1,8 +1,8 @@
 <?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="53"
-          android:versionName="9.1">
+    android:versionCode="51"
+          android:versionName="9.0">
 
   <uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />
 

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java

@@ -29,7 +29,6 @@
 
 package org.firstinspires.ftc.robotcontroller.external.samples;
 
-import android.util.Size;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -45,23 +44,6 @@ import java.util.List;
  * This OpMode illustrates the basics of AprilTag recognition and pose estimation,
  * including Java Builder structures for specifying Vision parameters.
  *
- * For an introduction to AprilTags, see the FTC-DOCS link below:
- * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
- *
- * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default
- * "TagLibrary" will have their position and orientation information displayed.  This default TagLibrary contains
- * the current Season's AprilTags and a small set of "test Tags" in the high number range.
- *
- * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
- * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
- * https://ftc-docs.firstinspires.org/apriltag-detection-values
- *
- * To experiment with using AprilTags to navigate, try out these two driving samples:
- * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank
- *
- * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired.
- * These default parameters are shown as comments in the code below.
- *
  * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
  * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
  */
@@ -124,8 +106,6 @@ public class ConceptAprilTag extends LinearOpMode {
 
         // Create the AprilTag processor.
         aprilTag = new AprilTagProcessor.Builder()
-
-            // The following default settings are available to un-comment and edit as needed.
             //.setDrawAxes(false)
             //.setDrawCubeProjection(false)
             //.setDrawTagOutline(true)
@@ -137,19 +117,11 @@ public class ConceptAprilTag extends LinearOpMode {
             // If you do not manually specify calibration parameters, the SDK will attempt
             // to load a predefined calibration for your camera.
             //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506)
+
             // ... these parameters are fx, fy, cx, cy.
 
             .build();
 
-        // Adjust Image Decimation to trade-off detection-range for detection-rate.
-        // eg: Some typical detection data using a Logitech C920 WebCam
-        // Decimation = 1 ..  Detect 2" Tag from 10 feet away at 10 Frames per second
-        // Decimation = 2 ..  Detect 2" Tag from 6  feet away at 22 Frames per second
-        // Decimation = 3 ..  Detect 2" Tag from 4  feet away at 30 Frames Per Second (default)
-        // Decimation = 3 ..  Detect 5" Tag from 10 feet away at 30 Frames Per Second (default)
-        // Note: Decimation can be changed on-the-fly to adapt during a match.
-        //aprilTag.setDecimation(3);
-
         // Create the vision portal by using a builder.
         VisionPortal.Builder builder = new VisionPortal.Builder();
 
@@ -164,7 +136,7 @@ public class ConceptAprilTag extends LinearOpMode {
         //builder.setCameraResolution(new Size(640, 480));
 
         // Enable the RC preview (LiveView).  Set "false" to omit camera monitoring.
-        //builder.enableLiveView(true);
+        //builder.enableCameraMonitoring(true);
 
         // Set the stream format; MJPEG uses less bandwidth than default YUY2.
         //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2);

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java

@@ -44,20 +44,6 @@ import java.util.List;
  * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using
  * the easy way.
  *
- * For an introduction to AprilTags, see the FTC-DOCS link below:
- * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
- *
- * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default
- * "TagLibrary" will have their position and orientation information displayed.  This default TagLibrary contains
- * the current Season's AprilTags and a small set of "test Tags" in the high number range.
- *
- * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
- * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
- * https://ftc-docs.firstinspires.org/apriltag-detection-values
- *
- * To experiment with using AprilTags to navigate, try out these two driving samples:
- * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank
- *
  * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
  * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
  */

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java

@@ -29,7 +29,6 @@
 
 package org.firstinspires.ftc.robotcontroller.external.samples;
 
-import android.util.Size;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -54,17 +53,6 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
 
     private static final boolean USE_WEBCAM = true;  // true for webcam, false for phone camera
 
-    // TFOD_MODEL_ASSET points to a model file stored in the project Asset location,
-    // this is only used for Android Studio when using models in Assets.
-    private static final String TFOD_MODEL_ASSET = "MyModelStoredAsAsset.tflite";
-    // TFOD_MODEL_FILE points to a model file stored onboard the Robot Controller's storage,
-    // this is used when uploading models directly to the RC using the model upload interface.
-    private static final String TFOD_MODEL_FILE = "/sdcard/FIRST/tflitemodels/myCustomModel.tflite";
-    // Define the labels recognized in the model for TFOD (must be in training order!)
-    private static final String[] LABELS = {
-       "Pixel",
-    };
-
     /**
      * The variable to store our instance of the TensorFlow Object Detection processor.
      */
@@ -119,16 +107,11 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
         // Create the TensorFlow processor by using a builder.
         tfod = new TfodProcessor.Builder()
 
-            // With the following lines commented out, the default TfodProcessor Builder
-            // will load the default model for the season. To define a custom model to load, 
-            // choose one of the following:
-            //   Use setModelAssetName() if the custom TF Model is built in as an asset (AS only).
-            //   Use setModelFileName() if you have downloaded a custom team model to the Robot Controller.
+            // Use setModelAssetName() if the TF Model is built in as an asset.
+            // Use setModelFileName() if you have downloaded a custom team model to the Robot Controller.
             //.setModelAssetName(TFOD_MODEL_ASSET)
             //.setModelFileName(TFOD_MODEL_FILE)
 
-            // The following default settings are available to un-comment and edit as needed to 
-            // set parameters for custom models.
             //.setModelLabels(LABELS)
             //.setIsModelTensorFlow2(true)
             //.setIsModelQuantized(true)
@@ -151,7 +134,7 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
         //builder.setCameraResolution(new Size(640, 480));
 
         // Enable the RC preview (LiveView).  Set "false" to omit camera monitoring.
-        //builder.enableLiveView(true);
+        //builder.enableCameraMonitoring(true);
 
         // Set the stream format; MJPEG uses less bandwidth than default YUY2.
         //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2);

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java

@@ -49,13 +49,6 @@ import java.util.concurrent.TimeUnit;
  * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
  * The code assumes a Holonomic (Mecanum or X Drive) Robot.
  *
- * For an introduction to AprilTags, see the ftc-docs link below:
- * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
- *
- * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
- * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
- * https://ftc-docs.firstinspires.org/apriltag-detection-values
- *
  * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and
  * driving towards the tag to achieve the desired distance.
  * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
@@ -109,7 +102,7 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
     private DcMotor rightBackDrive   = null;  //  Used to control the right back drive wheel
 
     private static final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
-    private static final int DESIRED_TAG_ID = -1;     // Choose the tag you want to approach or set to -1 for ANY tag.
+    private static final int DESIRED_TAG_ID = 0;     // Choose the tag you want to approach or set to -1 for ANY tag.
     private VisionPortal visionPortal;               // Used to manage the video source.
     private AprilTagProcessor aprilTag;              // Used for managing the AprilTag detection process.
     private AprilTagDetection desiredTag = null;     // Used to hold the data for a detected AprilTag
@@ -157,33 +150,25 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
             // Step through the list of detected tags and look for a matching tag
             List<AprilTagDetection> currentDetections = aprilTag.getDetections();
             for (AprilTagDetection detection : currentDetections) {
-                // Look to see if we have size info on this tag.
-                if (detection.metadata != null) {
-                    //  Check to see if we want to track towards this tag.
-                    if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
-                        // Yes, we want to use this tag.
-                        targetFound = true;
-                        desiredTag = detection;
-                        break;  // don't look any further.
-                    } else {
-                        // This tag is in the library, but we do not want to track it right now.
-                        telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
-                    }
+                if ((detection.metadata != null) &&
+                    ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID))  ){
+                    targetFound = true;
+                    desiredTag = detection;
+                    break;  // don't look any further.
                 } else {
-                    // This tag is NOT in the library, so we don't have enough information to track to it.
-                    telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+                    telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
                 }
             }
 
             // Tell the driver what we see, and what to do.
             if (targetFound) {
-                telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
-                telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+                telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
+                telemetry.addData("Target", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
                 telemetry.addData("Range",  "%5.1f inches", desiredTag.ftcPose.range);
                 telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
                 telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
             } else {
-                telemetry.addData("\n>","Drive using joysticks to find valid target\n");
+                telemetry.addData(">","Drive using joysticks to find valid target\n");
             }
 
             // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
@@ -258,15 +243,6 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
         // Create the AprilTag processor by using a builder.
         aprilTag = new AprilTagProcessor.Builder().build();
 
-        // Adjust Image Decimation to trade-off detection-range for detection-rate.
-        // eg: Some typical detection data using a Logitech C920 WebCam
-        // Decimation = 1 ..  Detect 2" Tag from 10 feet away at 10 Frames per second
-        // Decimation = 2 ..  Detect 2" Tag from 6  feet away at 22 Frames per second
-        // Decimation = 3 ..  Detect 2" Tag from 4  feet away at 30 Frames Per Second
-        // Decimation = 3 ..  Detect 5" Tag from 10 feet away at 30 Frames Per Second
-        // Note: Decimation can be changed on-the-fly to adapt during a match.
-        aprilTag.setDecimation(2);
-
         // Create the vision portal by using a builder.
         if (USE_WEBCAM) {
             visionPortal = new VisionPortal.Builder()

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java

@@ -49,13 +49,6 @@ import java.util.concurrent.TimeUnit;
  * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
  * The code assumes a basic two-wheel (Tank) Robot Drivetrain
  *
- * For an introduction to AprilTags, see the ftc-docs link below:
- * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
- *
- * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
- * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
- * https://ftc-docs.firstinspires.org/apriltag-detection-values
- *
  * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance.
  * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
  * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
@@ -104,7 +97,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
     private DcMotor rightDrive  = null;  //  Used to control the right drive wheel
 
     private static final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
-    private static final int DESIRED_TAG_ID = -1;    // Choose the tag you want to approach or set to -1 for ANY tag.
+    private static final int DESIRED_TAG_ID = 0;     // Choose the tag you want to approach or set to -1 for ANY tag.
     private VisionPortal visionPortal;               // Used to manage the video source.
     private AprilTagProcessor aprilTag;              // Used for managing the AprilTag detection process.
     private AprilTagDetection desiredTag = null;     // Used to hold the data for a detected AprilTag
@@ -147,32 +140,24 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
             // Step through the list of detected tags and look for a matching tag
             List<AprilTagDetection> currentDetections = aprilTag.getDetections();
             for (AprilTagDetection detection : currentDetections) {
-                // Look to see if we have size info on this tag.
-                if (detection.metadata != null) {
-                    //  Check to see if we want to track towards this tag.
-                    if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
-                        // Yes, we want to use this tag.
-                        targetFound = true;
-                        desiredTag = detection;
-                        break;  // don't look any further.
-                    } else {
-                        // This tag is in the library, but we do not want to track it right now.
-                        telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
-                    }
+                if ((detection.metadata != null) &&
+                    ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID))  ){
+                    targetFound = true;
+                    desiredTag = detection;
+                    break;  // don't look any further.
                 } else {
-                    // This tag is NOT in the library, so we don't have enough information to track to it.
-                    telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+                    telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
                 }
             }
 
             // Tell the driver what we see, and what to do.
             if (targetFound) {
-                telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
-                telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+                telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
+                telemetry.addData("Target", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
                 telemetry.addData("Range",  "%5.1f inches", desiredTag.ftcPose.range);
                 telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
             } else {
-                telemetry.addData("\n>","Drive using joysticks to find valid target\n");
+                telemetry.addData(">","Drive using joysticks to find valid target\n");
             }
 
             // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
@@ -233,15 +218,6 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
         // Create the AprilTag processor by using a builder.
         aprilTag = new AprilTagProcessor.Builder().build();
 
-        // Adjust Image Decimation to trade-off detection-range for detection-rate.
-        // eg: Some typical detection data using a Logitech C920 WebCam
-        // Decimation = 1 ..  Detect 2" Tag from 10 feet away at 10 Frames per second
-        // Decimation = 2 ..  Detect 2" Tag from 6  feet away at 22 Frames per second
-        // Decimation = 3 ..  Detect 2" Tag from 4  feet away at 30 Frames Per Second
-        // Decimation = 3 ..  Detect 5" Tag from 10 feet away at 30 Frames Per Second
-        // Note: Decimation can be changed on-the-fly to adapt during a match.
-        aprilTag.setDecimation(2);
-
         // Create the vision portal by using a builder.
         if (USE_WEBCAM) {
             visionPortal = new VisionPortal.Builder()

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java

@@ -1,78 +0,0 @@
-/* Copyright (c) 2024 FIRST. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted (subject to the limitations in the disclaimer below) provided that
- * the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of FIRST nor the names of its contributors may be used to endorse or
- * promote products derived from this software without specific prior written permission.
- *
- * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
- * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-package org.firstinspires.ftc.robotcontroller.external.samples;
-
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
-
-/*
- * This OpMode demonstrates how to use a digital channel.
- *
- * The OpMode assumes that the digital channel is configured with a name of "digitalTouch".
- *
- * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
- * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
- */
-@TeleOp(name = "Sensor: digital channel", group = "Sensor")
-@Disabled
-public class SensorDigitalTouch extends LinearOpMode {
-    DigitalChannel digitalTouch;  // Digital channel Object
-
-    @Override
-    public void runOpMode() {
-
-        // get a reference to our touchSensor object.
-        digitalTouch = hardwareMap.get(DigitalChannel.class, "digitalTouch");
-
-        digitalTouch.setMode(DigitalChannel.Mode.INPUT);
-        telemetry.addData("DigitalTouchSensorExample", "Press start to continue...");
-        telemetry.update();
-
-        // wait for the start button to be pressed.
-        waitForStart();
-
-        // while the OpMode is active, loop and read the digital channel.
-        // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
-        while (opModeIsActive()) {
-
-            // button is pressed if value returned is LOW or false.
-            // send the info back to driver station using telemetry function.
-            if (digitalTouch.getState() == false) {
-                telemetry.addData("Button", "PRESSED");
-            } else {
-                telemetry.addData("Button", "NOT PRESSED");
-            }
-
-            telemetry.update();
-        }
-    }
-}

README.md

@@ -59,58 +59,6 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc
 
 # Release Information
 
-## Version 9.1 (20240215-115542)
-
-### Enhancements
-* Fixes a problem with Blocks: if the user closes a Block's warning balloon, it will still be closed next time the project is opened in the Blocks editor.
-* In the Blocks editor, an alert concerning missing hardware devices is not shown if all the Blocks that use the missing hardware devices are disabled.
-* Adds Blocks to support comparing property values CRServo.Direction, DCMotor.Direction, DCMotor.Mode, DCMotor.ZeroPowerBehavior, DigitalChannel.Mode, GyroSensor.HeadingMode, IrSeekerSensor.Mode, and Servo.Direction, to the corresponding enum Block.
-* Improves OnBotJava auto-import to correctly import classes when used in certain situations.
-* Improves OnBotJava autocomplete to provide better completion options in most cases.
-  * This fixes an issue where autocomplete would fail if a method with two or more formal parameters was defined.
-* In OnBotJava, code folding support was added to expand and collapse code sections
-* In OnBotJava, the copyright header is now automatically collapsed loading new files
-* For all Blocks OpMode samples, intro comments have been moved to the RunOpMode comment balloon.
-* The Clean up Blocks command in the Blocks editor now positions function Blocks so their comment balloons don't overlap other function Blocks.
-* Added Blocks OpMode sample SensorTouch.
-* Added Java OpMode sample SensorDigitalTouch.
-* Several improvements to VisionPortal
-  * Adds option to control whether the stream is automatically started following a `.build()` call on a VisionPortal Builder
-  * Adds option to control whether the vision processing statistics overlay is rendered or not
-  * VisionPortals now implement the `CameraStreamSource` interface, allowing multiportal users to select which portal is routed to the DS in INIT by calling CameraStreamServer.getInstance().setSource(visionPortal). Can be selected via gamepad, between Camera Stream sessions.
-  * Add option to `AprilTagProcessor` to suppress calibration warnings
-  * Improves camera calibration warnings
-    * If a calibration is scaled, the resolution it was scaled from will be listed
-    * If calibrations exist with the wrong aspect ratio, the calibrated resolutions will be listed
-  * Fixes race condition which caused app crash when calling `stopStreaming()` immediately followed by `close()` on a VisionPortal
-  * Fixes IllegalStateException when calling `stopStreaming()` immediately after building a VisionPortal
-  * Added FTC Blocks counterparts to new Java methods:
-    * VisionPortal.Builder.setAutoStartStreamOnBuild
-    * VisionPortal.Builder.setShowStatsOverlay
-    * AprilTagProcessor.Builder.setSuppressCalibrationWarnings
-    * CameraStreamServer.setSource​
-
-### Bug Fixes
-* Fixes a problem where OnBotJava does not apply font size settings to the editor.
-* Updates EasyOpenCV dependency to v1.7.1
-  * Fixes inability to use EasyOpenCV CameraFactory in OnBotJava
-  * Fixes entire RC app crash when user pipeline throws an exception
-  * Fixes entire RC app crash when user user canvas annotator throws an exception
-  * Use the modern stacktrace display when handling user exceptions instead of the legacy ESTOP telemetry message
-
-## Version 9.0.1 (20230929-083754)
-
-### Enhancements
-* Updates AprilTag samples to include Decimation and additional Comments.  Also corrects misleading tag ID warnings
-* Increases maximum size of Blocks inline comments to 140 characters
-* Adds Blocks sample BasicOmniOpMode.
-* Updated CENTERSTAGE library AprilTag orientation quaternions
-    * Thanks [@FromenActual](https://github.com/FromenActual)
-* Updated Java Sample ConceptTensorFlowObjectDetection.java to include missing elements needed for custom model support.
-
-### Bug Fixes
-* Fixes a problem where after October 1 the Driver Station will report as obsolete on v9.0 and prompt the user to update.
-
 ## Version 9.0 (20230830-154348)
 
 ### Breaking Changes

TeamCode/build.gradle

@@ -26,4 +26,9 @@ android {
 dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
+    implementation 'org.ftclib.ftclib:core:2.0.1' // core
+    implementation 'org.apache.commons:commons-math3:3.6.1'
+    implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
+    implementation 'org.openftc:easyopencv:1.7.0'
+    implementation 'com.acmerobotics.roadrunner:core:0.5.6'
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java

@@ -0,0 +1,56 @@
+//package org.firstinspires.ftc.teamcode.hardware;
+//
+//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_LEFT;
+//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT;
+//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT;
+//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT;
+//
+//import com.qualcomm.robotcore.hardware.DcMotor;
+//import com.qualcomm.robotcore.hardware.HardwareMap;
+//
+//public class Drive {
+//    private DcMotor frontLeft;
+//    private DcMotor frontRight;
+//    private DcMotor backLeft;
+//    private DcMotor backRight;
+//
+//    public Drive(HardwareMap hardwareMap) {
+//        // Drive
+//        this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT);
+//        this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT);
+//        this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT);
+//        this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT);
+//        this.frontLeft.setDirection(DcMotor.Direction.REVERSE);
+//        this.frontRight.setDirection(DcMotor.Direction.FORWARD);
+//        this.backLeft.setDirection(DcMotor.Direction.REVERSE);
+//        this.backRight.setDirection(DcMotor.Direction.FORWARD);
+//        this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+//        this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+//        this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+//        this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+//    }
+//
+//    public void setInput(double x, double y, double z) {
+//        // instantiate motor power variables
+//        double flPower, frPower, blPower, brPower;
+//
+//        flPower = x + y + z;
+//        frPower = -x + y - z;
+//        blPower = -x + y + z;
+//        brPower = x + y - z;
+//
+//        double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower));
+//        if (max > 1) {
+//            flPower /= max;
+//            frPower /= max;
+//            blPower /= max;
+//            brPower /= max;
+//        }
+//
+//        // actually set the motor powers
+//        frontLeft.setPower(flPower);
+//        frontRight.setPower(frPower);
+//        backLeft.setPower(blPower);
+//        backRight.setPower(brPower);
+//    }
+//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java

@@ -0,0 +1,549 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import static org.firstinspires.ftc.teamcode.util.Constants.CLAW;
+import static org.firstinspires.ftc.teamcode.util.Constants.HANGLEFT;
+import static org.firstinspires.ftc.teamcode.util.Constants.HANGRIGHT;
+import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM;
+import static org.firstinspires.ftc.teamcode.util.Constants.LIGHTS;
+import static org.firstinspires.ftc.teamcode.util.Constants.PLANE;
+import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM;
+import static org.firstinspires.ftc.teamcode.util.Constants.SLIDELEFT;
+import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT;
+import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.arcrobotics.ftclib.controller.PController;
+import com.arcrobotics.ftclib.controller.PIDFController;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.vision.Camera;
+
+import lombok.Getter;
+
+@Config
+public class Robot {
+    public static boolean clawIsOpen;
+    public static double WRISTDELAY = .08;
+    public static int MIN_ERROR = 1;
+    public static int MAX_ERROR = 6;
+    public double distance = Double.MAX_VALUE;
+    public Pose2d estimatedPose;
+    public Pose2d drivePose;
+    public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
+    public armMacroStates armMacroState = armMacroStates.IDLE;
+    @Getter
+    public Arm arm;
+    double delay;
+    @Getter
+    private MecanumDrive drive;
+    @Getter
+    private Led led;
+    @Getter
+    private Wrist wrist;
+    @Getter
+    private Claw claw;
+    @Getter
+    private Hang hang;
+    @Getter
+    private Camera camera;
+    @Getter
+    private Plane plane;
+    @Getter
+    private Slides slides;
+
+    public Robot init(HardwareMap hardwareMap) {
+        this.drive = new MecanumDrive(hardwareMap);
+        this.hang = new Hang().init(hardwareMap);
+        this.arm = new Arm().init(hardwareMap);
+        this.wrist = new Wrist().init(hardwareMap);
+        this.claw = new Claw().init(hardwareMap);
+        this.camera = new Camera(hardwareMap);
+        this.plane = new Plane().init(hardwareMap);
+        this.slides = new Slides().init(hardwareMap);
+//        this.led = new Led().init(hardwareMap);
+        return this;
+    }
+
+    public void pickupMacro(GamepadEx gamepadEx, double runtime) {
+        switch (pickupMacroState) {
+            case IDLE:
+                if (gamepadEx.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) {
+                    pickupMacroState = pickupMacroStates.DROP;
+                }
+                break;
+            case OPEN:
+                delay = runtime + .3;
+                this.getClaw().open();
+                pickupMacroState = pickupMacroStates.DROP;
+                break;
+            case DROP:
+                if (runtime > delay) {
+                    this.getArm().pickup(true);
+                    delay = runtime + .2;
+                    pickupMacroState = pickupMacroStates.CLOSE;
+                }
+                break;
+            case CLOSE:
+                if (runtime > delay) {
+                    this.getClaw().close();
+                    delay = runtime + .3;
+                    pickupMacroState = pickupMacroStates.NEUTRAL;
+                }
+                break;
+            case NEUTRAL:
+                if (runtime > delay) {
+                    this.getArm().armRest(true);
+                    pickupMacroState = pickupMacroStates.IDLE;
+                }
+                break;
+
+        }
+    }
+
+    public void armMacro(GamepadEx gamepadEx, double runtime) {
+        switch (armMacroState) {
+            case IDLE:
+                if (gamepadEx.wasJustPressed(GamepadKeys.Button.X)) {
+                    armMacroState = armMacroStates.ARMSWING;
+                }
+                break;
+            case ARMSWING:
+                arm.armSecondaryScore();
+                delay = runtime + WRISTDELAY;
+                armMacroState = armMacroStates.WRIST;
+                break;
+            case WRIST:
+                if (runtime > delay) {
+                    wrist.wristScore();
+                    armMacroState = armMacroStates.IDLE;
+                }
+                break;
+        }
+    }
+
+    public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
+        this.drive.update();
+        return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
+    }
+
+    public void refreshPoseEstimateFromAprilTag() {
+        this.drive.update();
+        drivePose = this.drive.getPoseEstimate();
+        estimatedPose = this.camera.estimatePoseFromAprilTag();
+
+        if (estimatedPose != null) {
+            distance = Math.sqrt(Math.pow(drivePose.getX() - estimatedPose.getX(), 2) + Math.pow(drivePose.getY() - estimatedPose.getY(), 2));
+        }
+        if (distance < MAX_ERROR && distance > MIN_ERROR) {
+            this.drive.update(estimatedPose);
+        }
+    }
+
+    public void update() {
+        this.drive.update();
+        this.arm.update();
+        this.wrist.update();
+    }
+
+    public enum pickupMacroStates {
+        IDLE, OPEN, DROP, CLOSE, NEUTRAL
+
+    }
+
+    public enum armMacroStates {
+        IDLE, ARMSWING, WRIST
+    }
+
+    @Config
+    public static class Slides {
+        //Values
+        public static double SLIDE_POWER_UP = .7;
+        public static double SLIDE_POWER_DOWN = .2;
+        public static int SLIDELAYERONE = 150;
+        public static int SLIDEAUTOSTACKS = 250;
+        public static int SLIDEUP = 630;
+        public static int SLIDELAYERTWO = 350;
+        public static int SLIDESTACK = 80;
+        public static int SLIDEPICKUPSTACKSTWO = 30;
+        //Motors
+        public DcMotor slidesRight = null;
+        public DcMotor slidesLeft = null;
+
+        public Slides init(HardwareMap hardwareMap) {
+            this.slidesLeft = hardwareMap.get(DcMotor.class, SLIDELEFT);
+            this.slidesRight = hardwareMap.get(DcMotor.class, SLIDERIGHT);
+            this.slidesLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            this.slidesRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            this.slidesLeft.setTargetPosition(0);
+            this.slidesRight.setTargetPosition(0);
+
+            this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.slidesLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+            this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            return this;
+        }
+
+        public void slideTo(int position, double power) {
+            this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.slidesLeft.setTargetPosition(position);
+            this.slidesLeft.setPower(power);
+
+            this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.slidesRight.setTargetPosition(position);
+            this.slidesRight.setPower(power);
+        }
+
+        public void slideUp() {
+            this.slideTo(SLIDEUP, SLIDE_POWER_UP);
+        }
+
+        public void slideDown() {
+            this.slideTo(0, SLIDE_POWER_DOWN);
+        }
+
+        public void slideFirstLayer() {
+            this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);
+        }
+
+        public void slideScoreStack() {
+            this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);
+        }
+
+        public void slideAutoStacks() {
+            this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);
+        }
+
+        public void slidePickupStackTwo() {
+            this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);
+        }
+
+        public void slideStop() {
+            this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.slidesLeft.setTargetPosition(this.slidesLeft.getCurrentPosition());
+            this.slidesLeft.setPower(1);
+
+            this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.slidesRight.setTargetPosition(this.slidesRight.getCurrentPosition());
+            this.slidesRight.setPower(1);
+        }
+
+    }
+
+    @Config
+    public static class Hang {
+        //Values
+        public static int HANGRELEASE = 1550;
+        public static int HANG = 0;
+        public static int HANGPLANE = 1150;
+        //Motors
+        public DcMotor hangLeft = null;
+        public DcMotor hangRight = null;
+
+        public Hang init(HardwareMap hardwareMap) {
+            this.hangLeft = hardwareMap.get(DcMotor.class, HANGLEFT);
+            this.hangRight = hardwareMap.get(DcMotor.class, HANGRIGHT);
+            this.hangLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            this.hangRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            this.hangRight.setTargetPosition(0);
+            this.hangLeft.setTargetPosition(0);
+
+            this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.hangLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+            this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            return this;
+        }
+
+        public void hangTo(int hangPos, double Power) {
+            this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.hangLeft.setTargetPosition(hangPos);
+            this.hangLeft.setPower(Power);
+
+            this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            this.hangRight.setTargetPosition(hangPos);
+            this.hangRight.setPower(Power);
+        }
+
+        public void hangRelease() {
+            this.hangTo(HANGRELEASE, 1);
+        }
+
+        public void hang() {
+            this.hangTo(HANG, 1);
+        }
+
+        public void hangPlane() {
+            this.hangTo(HANGPLANE, 1);
+        }
+    }
+
+    @Config
+    public static class Arm {
+        //Values
+        public static double DIFFY_KP = 0.1;
+        public static double DIFFY_TOL = 0.01;
+        public static double ARM_MAX_DELTA = 0.02;
+        public static double ARMREST = 0.89;
+        public static double ARMSCORESTACK = 0.53;
+        public static double ARMACCSCORE = .57;
+        public static double ARMPICKUPSTACK = 0.91;
+        public static double ARMPICKUPSTACKLOW = 0.93;
+        public static double PICKUP = 0.92;
+        //PControler
+        public PController armPController;
+        private double armTarget;
+        //Servos
+        private Servo leftArm;
+        private Servo rightArm;
+
+        public Arm init(HardwareMap hardwareMap) {
+            this.leftArm = hardwareMap.get(Servo.class, LEFTARM);
+            this.rightArm = hardwareMap.get(Servo.class, RIGHTARM);
+            this.rightArm.setDirection(Servo.Direction.REVERSE);
+            this.rightArm.setPosition(ARMREST);
+            this.leftArm.setPosition(ARMREST);
+            this.armPController = new PController(DIFFY_KP);
+            this.armRest(true);
+            return this;
+        }
+
+        public void pickup() {
+            pickup(false);
+        }
+
+        public void pickup(boolean now) {
+            moveArm(PICKUP, now);
+        }
+
+        public void armPickdaUpy() {
+            moveArm(.83, false);
+        }
+
+
+        public void armScore() {
+            this.armSecondaryScore();
+        }
+
+        public void armSecondaryScore() {
+            moveArm(ARMACCSCORE, false);
+        }
+
+        public void armScoreStack() {
+            moveArm(ARMSCORESTACK, false);
+        }
+
+        public void armPickupStack() {
+            moveArm(ARMPICKUPSTACK, false);
+        }
+
+        public void armPickupStackLow() {
+            moveArm(ARMPICKUPSTACKLOW, false);
+        }
+
+
+        public void armRest() {
+            armRest(false);
+        }
+
+        public void armRest(boolean now) {
+            moveArm(ARMREST, now);
+        }
+
+        private void moveArm(double position, boolean now) {
+            this.armTarget = position;
+            this.armPController.setSetPoint(this.armTarget);
+            if (now) {
+                this.leftArm.setPosition(position);
+                this.rightArm.setPosition(position);
+            }
+        }
+
+        public boolean isAtTarget() {
+            return this.armPController.atSetPoint();
+        }
+
+        public void update() {
+            this.armPController.setSetPoint(this.armTarget);
+            this.armPController.setTolerance(DIFFY_TOL);
+            this.armPController.setP(DIFFY_KP);
+
+            if (!isAtTarget()) {
+                double armDelta = this.armPController.calculate(leftArm.getPosition());
+                if (Math.abs(this.armPController.getPositionError()) > 0.1) {
+                    armDelta = Math.copySign(ARM_MAX_DELTA, armDelta);
+                }
+                this.leftArm.setPosition(leftArm.getPosition() + armDelta);
+                this.rightArm.setPosition(rightArm.getPosition() + armDelta);
+            }
+
+//
+//            this.rightPController.setSetPoint(this.rightTarget);
+//            this.rightPController.setTolerance(DIFFY_TOL);
+//            this.rightPController.setP(DIFFY_KP);
+//            double rightDelta = this.rightPController.calculate(rightArm.getPosition());
+//            rightDelta = Math.max(-1 * DIFFY_MAX_DELTA, Math.min(DIFFY_MAX_DELTA, rightDelta));
+//            this.rightArm.setPosition(rightArm.getPosition() + rightDelta);
+
+            // A is where I am
+            // B is where I want to get to
+            // E is the difference between them
+            // X is the midpoint between A and B
+            // while I am at on the left side of X, delta should be constant
+            // once I am on the right side of X, delta should be whatever ther P controller says
+
+
+        }
+
+    }
+
+    @Config
+    public static class Wrist {
+        //Pcontroller
+        public static double KP = 0.2;
+        public static double TOL = 0.005;
+        public static double MAX_DELTA = 0.04;
+        //Values
+        public static double WRISTPICKUP = 0.3;
+        public static double WRISTSCORE = .98;
+        private PIDFController wristPController;
+        //Servo
+        private Servo wrist;
+
+        public Wrist init(HardwareMap hardwareMap) {
+            this.wrist = hardwareMap.get(Servo.class, WRIST);
+            this.wrist.setPosition(WRISTPICKUP);
+            this.wristPController = new PController(KP);
+            this.wristPController.setSetPoint(WRISTPICKUP);
+            return this;
+        }
+
+        public void wristPickup() {
+            this.wristPController.setSetPoint(WRISTPICKUP);
+        }
+
+        public void wristScore() {
+            this.wristPController.setSetPoint(WRISTSCORE);
+        }
+
+        public void update() {
+            this.wristPController.setTolerance(TOL);
+            this.wristPController.setP(KP);
+
+            if (!wristPController.atSetPoint()) {
+                double wristDelta = this.wristPController.calculate(wrist.getPosition());
+                if (Math.abs(this.wristPController.getPositionError()) > 0.1) {
+                    wristDelta = Math.copySign(MAX_DELTA, wristDelta);
+                }
+                this.wrist.setPosition(wrist.getPosition() + wristDelta);
+            }
+        }
+    }
+
+    @Config
+    public static class Claw {
+        //Values
+        public static double OPEN = 0.65;
+        public static double BIGOPEN = .42;
+        public static double CLOSE = 0.75;
+        public static double CLAW_MIN = 0;
+        public static double CLAW_MAX = 1;
+        //Servo
+        private Servo claw;
+
+        public Claw init(HardwareMap hardwareMap) {
+            this.claw = hardwareMap.get(Servo.class, CLAW);
+            this.claw.scaleRange(CLAW_MIN, CLAW_MAX);
+            close();
+            return this;
+        }
+
+        public void close() {
+            this.claw.setPosition(CLOSE);
+            clawIsOpen = false;
+        }
+
+        public void open() {
+            this.claw.setPosition(OPEN);
+            clawIsOpen = true;
+        }
+
+        public void openStack() {
+            this.claw.setPosition(BIGOPEN);
+            clawIsOpen = true;
+        }
+
+        public void setPos(double pos) {
+            this.claw.setPosition(pos);
+        }
+
+    }
+
+    @Config
+    public static class Led {
+        private RevBlinkinLedDriver led;
+
+        public Led init(HardwareMap hardwareMap) {
+            this.led = hardwareMap.get(RevBlinkinLedDriver.class, LIGHTS);
+            return this;
+        }
+
+        public void LED() {
+            if (clawIsOpen) {
+                white();
+            } else {
+                gold();
+            }
+        }
+
+        public void gold() {
+            this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_VIOLET);
+        }
+
+        public void white() {
+            this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.STROBE_WHITE);
+        }
+    }
+
+    @Config
+    public static class Plane {
+        //Values
+        public static double PLANELOCK = 0.1;
+        public static double PLANELAUNCH = 0.9;
+        //Servo
+        private Servo plane;
+
+        public Plane init(HardwareMap hardwareMap) {
+            this.plane = hardwareMap.get(Servo.class, PLANE);
+            this.plane.setPosition(PLANELOCK);
+            return this;
+        }
+
+        public void planeLock() {
+            this.plane.setPosition(PLANELOCK);
+        }
+
+        public void planeLaunch() {
+            this.plane.setPosition(PLANELAUNCH);
+        }
+
+    }
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java

@@ -0,0 +1,97 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+/*
+ * Constants shared between multiple drive types.
+ *
+ * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
+ * fields may also be edited through the dashboard (connect to the robot's WiFi network and
+ * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
+ *
+ * These are not the only parameters; some are located in the localizer classes, drive base classes,
+ * and op modes themselves.
+ */
+@Config
+public class DriveConstants {
+
+    /*
+     * These are motor constants that should be listed online for your motors.
+     */
+    public static final double TICKS_PER_REV = 384.5;
+    public static final double MAX_RPM = 435;
+
+    /*
+     * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
+     * Set this flag to false if drive encoders are not present and an alternative localization
+     * method is in use (e.g., tracking wheels).
+     *
+     * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
+     * from DriveVelocityPIDTuner.
+     */
+    public static final boolean RUN_USING_ENCODER = false;
+    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
+            getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+
+    /*
+     * These are physical constants that can be determined from your robot (including the track
+     * width; it will be tune empirically later although a rough estimate is important). Users are
+     * free to chose whichever linear distance unit they would like so long as it is consistently
+     * used. The default values were selected with inches in mind. Road runner uses radians for
+     * angular distances although most angular parameters are wrapped in Math.toRadians() for
+     * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
+     */
+    public static double WHEEL_RADIUS = 1.88976; // in
+    public static double GEAR_RATIO = 24f / 16f; // output (wheel) speed / input (motor) speed
+    public static double TRACK_WIDTH = 12.438; // in
+
+    //    public static double WHEEL_RADIUS = 1.88976; // in
+//    public static double GEAR_RATIO = 652.5/435; // output (wheel) speed / input (motor) speed
+//    public static double TRACK_WIDTH = 13; // in
+    /*
+     * These are the feedforward parameters used to model the drive motor behavior. If you are using
+     * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
+     * motor encoders or have elected not to use them for velocity control, these values should be
+     * empirically tuned.
+     */
+    public static double kV = 1 / rpmToVelocity(MAX_RPM);
+    public static double kA = 0;
+    public static double kStatic = 0;
+
+    /*
+     * These values are used to generate the trajectories for you robot. To ensure proper operation,
+     * the constraints should never exceed ~80% of the robot's actual capabilities. While Road
+     * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
+     * small and gradually increase them later after everything is working. All distance units are
+     * inches.
+     */
+    public static double MAX_VEL = 60;
+    public static double MAX_ACCEL = 50;
+    public static double MAX_ANG_VEL = Math.toRadians(360);
+    public static double MAX_ANG_ACCEL = Math.toRadians(360);
+
+    /*
+     * Adjust the orientations here to match your robot. See the FTC SDK documentation for details.
+     */
+    public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR =
+            RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
+    public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR =
+            RevHubOrientationOnRobot.UsbFacingDirection.UP;
+
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    public static double rpmToVelocity(double rpm) {
+        return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
+    }
+
+    public static double getMotorVelocityF(double ticksPerSecond) {
+        // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
+        return 32767 / ticksPerSecond;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java

@@ -0,0 +1,349 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
+
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_SPEED;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_TURN;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SPEED;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.TURN;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+
+
+/*
+ * Simple mecanum drive hardware implementation for REV hardware.
+ */
+@Config
+public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 4);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2.5);
+
+    public static double LATERAL_MULTIPLIER = 1;
+
+    public static double VX_WEIGHT = 1;
+
+    public static double VY_WEIGHT = 1;
+    public static double OMEGA_WEIGHT = 1;
+
+    private TrajectorySequenceRunner trajectorySequenceRunner;
+
+    private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+    private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
+
+    private TrajectoryFollower follower;
+
+    public DcMotorEx leftFront, leftRear, rightRear, rightFront;
+    private List<DcMotorEx> motors;
+
+    private IMU imu;
+    private VoltageSensor batteryVoltageSensor;
+
+    private List<Integer> lastEncPositions = new ArrayList<>();
+    private List<Integer> lastEncVels = new ArrayList<>();
+
+    public MecanumDrive(HardwareMap hardwareMap) {
+        super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
+
+        follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
+                new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+        LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+
+        // TODO: adjust the names of the following hardware devices to match your configuration
+        imu = hardwareMap.get(IMU.class, "imu");
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR));
+        imu.initialize(parameters);
+
+
+        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+        leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
+        rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
+        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+
+
+        this.leftFront.setDirection(DcMotor.Direction.REVERSE);
+        this.leftRear.setDirection(DcMotor.Direction.REVERSE);
+        this.rightFront.setDirection(DcMotor.Direction.FORWARD);
+        this.rightRear.setDirection(DcMotor.Direction.FORWARD);
+        this.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+        if (RUN_USING_ENCODER) {
+            setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        }
+
+        setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+            setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+        }
+
+        // TODO: reverse any motors using DcMotor.setDirection()
+
+        List<Integer> lastTrackingEncPositions = new ArrayList<>();
+        List<Integer> lastTrackingEncVels = new ArrayList<>();
+
+        // TODO: if desired, use setLocalizer() to change the localization method
+         setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
+
+        trajectorySequenceRunner = new TrajectorySequenceRunner(
+                follower, HEADING_PID, batteryVoltageSensor,
+                lastEncPositions, lastEncVels, lastTrackingEncPositions, lastTrackingEncVels
+        );
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+        return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+        return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+        return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+        return new TrajectorySequenceBuilder(
+                startPose,
+                VEL_CONSTRAINT, ACCEL_CONSTRAINT,
+                MAX_ANG_VEL, MAX_ANG_ACCEL
+        );
+    }
+
+    public void turnAsync(double angle) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(
+                trajectorySequenceBuilder(getPoseEstimate())
+                        .turn(angle)
+                        .build()
+        );
+    }
+
+    public void turn(double angle) {
+        turnAsync(angle);
+        waitForIdle();
+    }
+
+    public void followTrajectoryAsync(Trajectory trajectory) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(
+                trajectorySequenceBuilder(trajectory.start())
+                        .addTrajectory(trajectory)
+                        .build()
+        );
+    }
+
+    public void followTrajectory(Trajectory trajectory) {
+        followTrajectoryAsync(trajectory);
+        waitForIdle();
+    }
+
+    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+        trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+    }
+
+    public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+        followTrajectorySequenceAsync(trajectorySequence);
+        waitForIdle();
+    }
+
+    public Pose2d getLastError() {
+        return trajectorySequenceRunner.getLastPoseError();
+    }
+
+    public void update() {
+        this.update(null);
+    }
+
+    public void update(Pose2d guess) {
+        if (guess == null) {
+            updatePoseEstimate();
+        } else {
+            setPoseEstimate(guess);
+        }
+        DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+        if (signal != null) setDriveSignal(signal);
+    }
+
+    public void waitForIdle() {
+        while (!Thread.currentThread().isInterrupted() && isBusy())
+            update();
+    }
+
+    public boolean isBusy() {
+        return trajectorySequenceRunner.isBusy();
+    }
+
+    public void setMode(DcMotor.RunMode runMode) {
+        for (DcMotorEx motor : motors) {
+            motor.setMode(runMode);
+        }
+    }
+
+    public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+        for (DcMotorEx motor : motors) {
+            motor.setZeroPowerBehavior(zeroPowerBehavior);
+        }
+    }
+
+    public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+        PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+                coefficients.p, coefficients.i, coefficients.d,
+                coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+        );
+
+        for (DcMotorEx motor : motors) {
+            motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+        }
+    }
+
+    public void setWeightedDrivePower(Pose2d drivePower) {
+        Pose2d vel = drivePower;
+
+        if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
+                + Math.abs(drivePower.getHeading()) > 1) {
+            // re-normalize the powers according to the weights
+            double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+                    + VY_WEIGHT * Math.abs(drivePower.getY())
+                    + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+            vel = new Pose2d(
+                    VX_WEIGHT * drivePower.getX(),
+                    VY_WEIGHT * drivePower.getY(),
+                    OMEGA_WEIGHT * drivePower.getHeading()
+            ).div(denom);
+        }
+
+        setDrivePower(vel);
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        lastEncPositions.clear();
+
+        List<Double> wheelPositions = new ArrayList<>();
+        for (DcMotorEx motor : motors) {
+            int position = motor.getCurrentPosition();
+            lastEncPositions.add(position);
+            wheelPositions.add(encoderTicksToInches(position));
+        }
+        return wheelPositions;
+    }
+
+    @Override
+    public List<Double> getWheelVelocities() {
+        lastEncVels.clear();
+
+        List<Double> wheelVelocities = new ArrayList<>();
+        for (DcMotorEx motor : motors) {
+            int vel = (int) motor.getVelocity();
+            lastEncVels.add(vel);
+            wheelVelocities.add(encoderTicksToInches(vel));
+        }
+        return wheelVelocities;
+    }
+
+    @Override
+    public void setMotorPowers(double v, double v1, double v2, double v3) {
+        leftFront.setPower(v);
+        leftRear.setPower(v1);
+        rightRear.setPower(v2);
+        rightFront.setPower(v3);
+    }
+
+    @Override
+    public double getRawExternalHeading() {
+        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    @Override
+    public Double getExternalHeadingVelocity() {
+        return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).xRotationRate;
+    }
+
+    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+        return new MinVelocityConstraint(Arrays.asList(
+                new AngularVelocityConstraint(maxAngularVel),
+                new MecanumVelocityConstraint(maxVel, trackWidth)
+        ));
+    }
+
+    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+        return new ProfileAccelerationConstraint(maxAccel);
+    }
+
+    public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
+        double speedScale = gamepad1.y || gamepad1.right_bumper ? SLOWMODE_SPEED : SPEED;
+        double turnScale = gamepad1.y || gamepad1.right_bumper ? SLOWMODE_TURN : TURN;
+
+        this.setWeightedDrivePower(
+                new Pose2d(
+                        -gamepad1.left_stick_y * speedScale,
+                        -gamepad1.left_stick_x * speedScale,
+                        -gamepad1.right_stick_x * turnScale
+                ));
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java

@@ -0,0 +1,100 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder;
+
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |
+ *    | ||        || |
+ *    | ||        || |
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *
+ */
+@Config
+public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
+    public static double TICKS_PER_REV = 2000;
+    public static double WHEEL_RADIUS = 0.944882; // in
+    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+
+    public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels
+    public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel
+
+    private Encoder leftEncoder, rightEncoder, frontEncoder;
+
+    private List<Integer> lastEncPositions, lastEncVels;
+
+    public StandardTrackingWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
+        super(Arrays.asList(
+                new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
+                new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
+                new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
+        ));
+
+        lastEncPositions = lastTrackingEncPositions;
+        lastEncVels = lastTrackingEncVels;
+
+        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder"));
+        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder"));
+        frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder"));
+
+        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+    }
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        int leftPos = leftEncoder.getCurrentPosition();
+        int rightPos = rightEncoder.getCurrentPosition();
+        int frontPos = frontEncoder.getCurrentPosition();
+
+        lastEncPositions.clear();
+        lastEncPositions.add(leftPos);
+        lastEncPositions.add(rightPos);
+        lastEncPositions.add(frontPos);
+
+        return Arrays.asList(
+                encoderTicksToInches(leftPos),
+                encoderTicksToInches(rightPos),
+                encoderTicksToInches(frontPos)
+        );
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelVelocities() {
+        int leftVel = (int) leftEncoder.getCorrectedVelocity();
+        int rightVel = (int) rightEncoder.getCorrectedVelocity();
+        int frontVel = (int) frontEncoder.getCorrectedVelocity();
+
+        lastEncVels.clear();
+        lastEncVels.add(leftVel);
+        lastEncVels.add(rightVel);
+        lastEncVels.add(frontVel);
+
+        return Arrays.asList(
+                encoderTicksToInches(leftVel),
+                encoderTicksToInches(rightVel),
+                encoderTicksToInches(frontVel)
+        );
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java

@@ -0,0 +1,107 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder;
+
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ *    ^
+ *    |
+ *    | ( x direction)
+ *    |
+ *    v
+ *    <----( y direction )---->
+
+ *        (forward)
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |    <- Perpendicular Wheel
+ *    |           || |
+ *    |           || |    <- Parallel Wheel
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *
+ */
+public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
+    public static double TICKS_PER_REV = 2000;
+    public static double WHEEL_RADIUS = 0.944882; // in
+    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+
+    public static double PARALLEL_X = 4.84; // X is the up and down direction
+    public static double PARALLEL_Y = 3.94; // Y is the strafe direction
+
+    public static double PERPENDICULAR_X = 5.10;
+    public static double PERPENDICULAR_Y = 0.51;
+
+    public static double X_MULTIPLIER = 1; // Multiplier in the X direction
+    public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction
+    // Parallel/Perpendicular to the forward axis
+    // Parallel wheel is parallel to the forward axis
+    // Perpendicular is perpendicular to the forward axis
+    private final Encoder parallelEncoder;
+    private final Encoder perpendicularEncoder;
+
+    private final MecanumDrive drive;
+
+    public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, MecanumDrive drive) {
+        super(Arrays.asList(
+                new Pose2d(PARALLEL_X, PARALLEL_Y, 0),
+                new Pose2d(PERPENDICULAR_X, PERPENDICULAR_Y, Math.toRadians(90))
+        ));
+
+        this.drive = drive;
+
+        parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront"));
+        perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear"));
+
+        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+//        parallelEncoder.setDirection(Encoder.Direction.REVERSE);
+    }
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    @Override
+    public double getHeading() {
+        return drive.getRawExternalHeading();
+    }
+
+    @Override
+    public Double getHeadingVelocity() {
+        return drive.getExternalHeadingVelocity();
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        return Arrays.asList(
+                encoderTicksToInches(parallelEncoder.getCurrentPosition()) * X_MULTIPLIER,
+                encoderTicksToInches(perpendicularEncoder.getCurrentPosition()) * Y_MULTIPLIER
+        );
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelVelocities() {
+        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
+        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
+        //  compensation method
+
+        return Arrays.asList(
+                encoderTicksToInches(parallelEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
+                encoderTicksToInches(perpendicularEncoder.getCorrectedVelocity()) * Y_MULTIPLIER
+        );
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java

@@ -0,0 +1,223 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_RPM;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.rpmToVelocity;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LoggingUtil;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.RegressionUtil;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/*
+ * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
+ * outline of the procedure:
+ *   1. Slowly ramp the motor power and record encoder values along the way.
+ *   2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
+ *      and an optional intercept (kStatic).
+ *   3. Accelerate the robot (apply constant power) and record the encoder counts.
+ *   4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
+ *      regression.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class AutomaticFeedforwardTuner extends LinearOpMode {
+    public static double MAX_POWER = 0.7;
+    public static double DISTANCE = 100; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (RUN_USING_ENCODER) {
+            RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+                    "when using the built-in drive motor velocity PID.");
+        }
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        NanoClock clock = NanoClock.system();
+
+        telemetry.addLine("Press play to begin the feedforward tuning routine");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Would you like to fit kStatic?");
+        telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+        telemetry.update();
+
+        boolean fitIntercept = false;
+        while (!isStopRequested()) {
+            if (gamepad1.y) {
+                fitIntercept = true;
+                while (!isStopRequested() && gamepad1.y) {
+                    idle();
+                }
+                break;
+            } else if (gamepad1.b) {
+                while (!isStopRequested() && gamepad1.b) {
+                    idle();
+                }
+                break;
+            }
+            idle();
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine(Misc.formatInvariant(
+                "Place your robot on the field with at least %.2f in of room in front", DISTANCE));
+        telemetry.addLine("Press (Y/Δ) to begin");
+        telemetry.update();
+
+        while (!isStopRequested() && !gamepad1.y) {
+            idle();
+        }
+        while (!isStopRequested() && gamepad1.y) {
+            idle();
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        double maxVel = rpmToVelocity(MAX_RPM);
+        double finalVel = MAX_POWER * maxVel;
+        double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
+        double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
+
+        List<Double> timeSamples = new ArrayList<>();
+        List<Double> positionSamples = new ArrayList<>();
+        List<Double> powerSamples = new ArrayList<>();
+
+        drive.setPoseEstimate(new Pose2d());
+
+        double startTime = clock.seconds();
+        while (!isStopRequested()) {
+            double elapsedTime = clock.seconds() - startTime;
+            if (elapsedTime > rampTime) {
+                break;
+            }
+            double vel = accel * elapsedTime;
+            double power = vel / maxVel;
+
+            timeSamples.add(elapsedTime);
+            positionSamples.add(drive.getPoseEstimate().getX());
+            powerSamples.add(power);
+
+            drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
+            drive.updatePoseEstimate();
+        }
+        drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+        RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
+                timeSamples, positionSamples, powerSamples, fitIntercept,
+                LoggingUtil.getLogFile(Misc.formatInvariant(
+                        "DriveRampRegression-%d.csv", System.currentTimeMillis())));
+
+        telemetry.clearAll();
+        telemetry.addLine("Quasi-static ramp up test complete");
+        if (fitIntercept) {
+            telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
+                    rampResult.kV, rampResult.kStatic, rampResult.rSquare));
+        } else {
+            telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
+                    rampResult.kStatic, rampResult.rSquare));
+        }
+        telemetry.addLine("Would you like to fit kA?");
+        telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+        telemetry.update();
+
+        boolean fitAccelFF = false;
+        while (!isStopRequested()) {
+            if (gamepad1.y) {
+                fitAccelFF = true;
+                while (!isStopRequested() && gamepad1.y) {
+                    idle();
+                }
+                break;
+            } else if (gamepad1.b) {
+                while (!isStopRequested() && gamepad1.b) {
+                    idle();
+                }
+                break;
+            }
+            idle();
+        }
+
+        if (fitAccelFF) {
+            telemetry.clearAll();
+            telemetry.addLine("Place the robot back in its starting position");
+            telemetry.addLine("Press (Y/Δ) to continue");
+            telemetry.update();
+
+            while (!isStopRequested() && !gamepad1.y) {
+                idle();
+            }
+            while (!isStopRequested() && gamepad1.y) {
+                idle();
+            }
+
+            telemetry.clearAll();
+            telemetry.addLine("Running...");
+            telemetry.update();
+
+            double maxPowerTime = DISTANCE / maxVel;
+
+            timeSamples.clear();
+            positionSamples.clear();
+            powerSamples.clear();
+
+            drive.setPoseEstimate(new Pose2d());
+            drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
+
+            startTime = clock.seconds();
+            while (!isStopRequested()) {
+                double elapsedTime = clock.seconds() - startTime;
+                if (elapsedTime > maxPowerTime) {
+                    break;
+                }
+
+                timeSamples.add(elapsedTime);
+                positionSamples.add(drive.getPoseEstimate().getX());
+                powerSamples.add(MAX_POWER);
+
+                drive.updatePoseEstimate();
+            }
+            drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+            RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
+                    timeSamples, positionSamples, powerSamples, rampResult,
+                    LoggingUtil.getLogFile(Misc.formatInvariant(
+                            "DriveAccelRegression-%d.csv", System.currentTimeMillis())));
+
+            telemetry.clearAll();
+            telemetry.addLine("Constant power test complete");
+            telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
+                    accelResult.kA, accelResult.rSquare));
+            telemetry.update();
+        }
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/BackAndForth.java

@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin moving forward and
+ * backward. You should observe the target position (green) and your pose estimate (blue) and adjust
+ * your follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ *
+ * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
+ * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class BackAndForth extends LinearOpMode {
+
+    public static double DISTANCE = 50;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
+                .forward(DISTANCE)
+                .build();
+
+        Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
+                .back(DISTANCE)
+                .build();
+
+        waitForStart();
+
+        while (opModeIsActive() && !isStopRequested()) {
+            drive.followTrajectory(trajectoryForward);
+            drive.followTrajectory(trajectoryBackward);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/DriveVelocityPIDTuner.java

@@ -0,0 +1,173 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+import java.util.List;
+
+/*
+ * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
+ * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
+ * important as the positional parameters. Like the other manual tuning routines, this op mode
+ * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
+ * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
+ * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
+ * connected, start the program, and your robot will begin moving forward and backward according to
+ * a motion profile. Your job is to graph the velocity errors over time and adjust the PID
+ * coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
+ * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
+ * MOTOR_VELO_PID field.
+ *
+ * Recommended tuning process:
+ *
+ * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
+ *    mitigate oscillations.
+ * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
+ * 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class DriveVelocityPIDTuner extends LinearOpMode {
+    public static double DISTANCE = 72; // in
+
+    enum Mode {
+        DRIVER_MODE,
+        TUNING_MODE
+    }
+
+    private static MotionProfile generateProfile(boolean movingForward) {
+        MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+        MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+        return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+    }
+
+    @Override
+    public void runOpMode() {
+        if (!RUN_USING_ENCODER) {
+            RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
+                    "PID is not in use", getClass().getSimpleName());
+        }
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        Mode mode = Mode.TUNING_MODE;
+
+        double lastKp = MOTOR_VELO_PID.p;
+        double lastKi = MOTOR_VELO_PID.i;
+        double lastKd = MOTOR_VELO_PID.d;
+        double lastKf = MOTOR_VELO_PID.f;
+
+        drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+        NanoClock clock = NanoClock.system();
+
+        telemetry.addLine("Ready!");
+        telemetry.update();
+        telemetry.clearAll();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        boolean movingForwards = true;
+        MotionProfile activeProfile = generateProfile(true);
+        double profileStart = clock.seconds();
+
+
+        while (!isStopRequested()) {
+            telemetry.addData("mode", mode);
+
+            switch (mode) {
+                case TUNING_MODE:
+                    if (gamepad1.y) {
+                        mode = Mode.DRIVER_MODE;
+                        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                    }
+
+                    // calculate and set the motor power
+                    double profileTime = clock.seconds() - profileStart;
+
+                    if (profileTime > activeProfile.duration()) {
+                        // generate a new profile
+                        movingForwards = !movingForwards;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    MotionState motionState = activeProfile.get(profileTime);
+                    double targetPower = kV * motionState.getV();
+                    drive.setDrivePower(new Pose2d(targetPower, 0, 0));
+
+                    List<Double> velocities = drive.getWheelVelocities();
+
+                    // update telemetry
+                    telemetry.addData("targetVelocity", motionState.getV());
+                    for (int i = 0; i < velocities.size(); i++) {
+                        telemetry.addData("measuredVelocity" + i, velocities.get(i));
+                        telemetry.addData(
+                                "error" + i,
+                                motionState.getV() - velocities.get(i)
+                        );
+                    }
+                    break;
+                case DRIVER_MODE:
+                    if (gamepad1.b) {
+                        drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+                        mode = Mode.TUNING_MODE;
+                        movingForwards = true;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    drive.setWeightedDrivePower(
+                            new Pose2d(
+                                    -gamepad1.left_stick_y,
+                                    -gamepad1.left_stick_x,
+                                    -gamepad1.right_stick_x
+                            )
+                    );
+                    break;
+            }
+
+            if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
+                    || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
+                drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+                lastKp = MOTOR_VELO_PID.p;
+                lastKi = MOTOR_VELO_PID.i;
+                lastKd = MOTOR_VELO_PID.d;
+                lastKf = MOTOR_VELO_PID.f;
+            }
+
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/FollowerPIDTuner.java

@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequence;
+
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin driving in a square.
+ * You should observe the target position (green) and your pose estimate (blue) and adjust your
+ * follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class FollowerPIDTuner extends LinearOpMode {
+    public static double DISTANCE = 48; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
+
+        drive.setPoseEstimate(startPose);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (!isStopRequested()) {
+            TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .build();
+            drive.followTrajectorySequence(trajSeq);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/LocalizationTest.java

@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/**
+ * This is a simple teleop routine for testing localization. Drive the robot around like a normal
+ * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
+ * errors are not out of the ordinary, especially with sudden drive motions). The goal of this
+ * exercise is to ascertain whether the localizer has been configured properly (note: the pure
+ * encoder localizer heading may be significantly off if the track width has not been tuned).
+ */
+@Disabled
+@TeleOp(group = "drive")
+public class LocalizationTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        waitForStart();
+
+        while (!isStopRequested()) {
+            drive.setWeightedDrivePower(
+                    new Pose2d(
+                            -gamepad1.left_stick_y,
+                            -gamepad1.left_stick_x,
+                            -gamepad1.right_stick_x
+                    )
+            );
+
+            drive.update();
+
+            Pose2d poseEstimate = drive.getPoseEstimate();
+            telemetry.addData("x", poseEstimate.getX());
+            telemetry.addData("y", poseEstimate.getY());
+            telemetry.addData("heading", poseEstimate.getHeading());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/ManualFeedforwardTuner.java

@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+import java.util.Objects;
+
+/*
+ * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
+ * tuning these coefficients is just as important as the positional parameters. Like the other
+ * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
+ * you are using the Control Hub. Once you've successfully connected, start the program, and your
+ * robot will begin moving forward and backward according to a motion profile. Your job is to graph
+ * the velocity errors over time and adjust the feedforward coefficients. Once you've found a
+ * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class ManualFeedforwardTuner extends LinearOpMode {
+    public static double DISTANCE = 72; // in
+
+    private FtcDashboard dashboard = FtcDashboard.getInstance();
+
+    private MecanumDrive drive;
+
+    enum Mode {
+        DRIVER_MODE,
+        TUNING_MODE
+    }
+
+    private Mode mode;
+
+    private static MotionProfile generateProfile(boolean movingForward) {
+        MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+        MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+        return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+    }
+
+    @Override
+    public void runOpMode() {
+        if (RUN_USING_ENCODER) {
+            RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+                    "when using the built-in drive motor velocity PID.");
+        }
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
+
+        drive = new MecanumDrive(hardwareMap);
+
+        final VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        mode = Mode.TUNING_MODE;
+
+        NanoClock clock = NanoClock.system();
+
+        telemetry.addLine("Ready!");
+        telemetry.update();
+        telemetry.clearAll();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        boolean movingForwards = true;
+        MotionProfile activeProfile = generateProfile(true);
+        double profileStart = clock.seconds();
+
+
+        while (!isStopRequested()) {
+            telemetry.addData("mode", mode);
+
+            switch (mode) {
+                case TUNING_MODE:
+                    if (gamepad1.y) {
+                        mode = Mode.DRIVER_MODE;
+                    }
+
+                    // calculate and set the motor power
+                    double profileTime = clock.seconds() - profileStart;
+
+                    if (profileTime > activeProfile.duration()) {
+                        // generate a new profile
+                        movingForwards = !movingForwards;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    MotionState motionState = activeProfile.get(profileTime);
+                    double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
+
+                    final double NOMINAL_VOLTAGE = 12.0;
+                    final double voltage = voltageSensor.getVoltage();
+                    drive.setDrivePower(new Pose2d(NOMINAL_VOLTAGE / voltage * targetPower, 0, 0));
+                    drive.updatePoseEstimate();
+
+                    Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+                    double currentVelo = poseVelo.getX();
+
+                    // update telemetry
+                    telemetry.addData("targetVelocity", motionState.getV());
+                    telemetry.addData("measuredVelocity", currentVelo);
+                    telemetry.addData("error", motionState.getV() - currentVelo);
+                    break;
+                case DRIVER_MODE:
+                    if (gamepad1.b) {
+                        mode = Mode.TUNING_MODE;
+                        movingForwards = true;
+                        activeProfile = generateProfile(movingForwards);
+                        profileStart = clock.seconds();
+                    }
+
+                    drive.setWeightedDrivePower(
+                            new Pose2d(
+                                    -gamepad1.left_stick_y,
+                                    -gamepad1.left_stick_x,
+                                    -gamepad1.right_stick_x
+                            )
+                    );
+                    break;
+            }
+
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxAngularVeloTuner.java

@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
+ * <p>
+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds.
+ * <p>
+ * Further fine tuning of MAX_ANG_VEL may be desired.
+ */
+
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class MaxAngularVeloTuner extends LinearOpMode {
+    public static double RUNTIME = 4.0;
+
+    private ElapsedTime timer;
+    private double maxAngVelocity = 0.0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
+        telemetry.addLine("Please ensure you have enough space cleared.");
+        telemetry.addLine("");
+        telemetry.addLine("Press start when ready.");
+        telemetry.update();
+
+        waitForStart();
+
+        telemetry.clearAll();
+        telemetry.update();
+
+        drive.setDrivePower(new Pose2d(0, 0, 1));
+        timer = new ElapsedTime();
+
+        while (!isStopRequested() && timer.seconds() < RUNTIME) {
+            drive.updatePoseEstimate();
+
+            Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+            maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
+        }
+
+        drive.setDrivePower(new Pose2d());
+
+        telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
+        telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
+        telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
+        telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
+        telemetry.update();
+
+        while (!isStopRequested()) idle();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxVelocityTuner.java

@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum velocity your bot can achieve under load. It
+ * will also calculate the effective kF value for your velocity PID.
+ * <p>
+ * Upon pressing start, your bot will run at max power for RUNTIME seconds.
+ * <p>
+ * Further fine tuning of kF may be desired.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class MaxVelocityTuner extends LinearOpMode {
+    public static double RUNTIME = 2.0;
+
+    private ElapsedTime timer;
+    private double maxVelocity = 0.0;
+
+    private VoltageSensor batteryVoltageSensor;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
+        telemetry.addLine("Please ensure you have enough space cleared.");
+        telemetry.addLine("");
+        telemetry.addLine("Press start when ready.");
+        telemetry.update();
+
+        waitForStart();
+
+        telemetry.clearAll();
+        telemetry.update();
+
+        drive.setDrivePower(new Pose2d(1, 0, 0));
+        timer = new ElapsedTime();
+
+        while (!isStopRequested() && timer.seconds() < RUNTIME) {
+            drive.updatePoseEstimate();
+
+            Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+            maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
+        }
+
+        drive.setDrivePower(new Pose2d());
+
+        double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
+
+        telemetry.addData("Max Velocity", maxVelocity);
+        telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
+        telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
+        telemetry.update();
+
+        while (!isStopRequested() && opModeIsActive()) idle();
+    }
+
+    private double veloInchesToTicks(double inchesPerSec) {
+        return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MotorDirectionDebugger.java

@@ -0,0 +1,94 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/**
+ * This is a simple teleop routine for debugging your motor configuration.
+ * Pressing each of the buttons will power its respective motor.
+ *
+ * Button Mappings:
+ *
+ * Xbox/PS4 Button - Motor
+ *   X / ▢         - Front Left
+ *   Y / Δ         - Front Right
+ *   B / O         - Rear  Right
+ *   A / X         - Rear  Left
+ *                                    The buttons are mapped to match the wheels spatially if you
+ *                                    were to rotate the gamepad 45deg°. x/square is the front left
+ *                    ________        and each button corresponds to the wheel as you go clockwise
+ *                   / ______ \
+ *     ------------.-'   _  '-..+              Front of Bot
+ *              /   _  ( Y )  _  \                  ^
+ *             |  ( X )  _  ( B ) |     Front Left   \    Front Right
+ *        ___  '.      ( A )     /|       Wheel       \      Wheel
+ *      .'    '.    '-._____.-'  .'       (x/▢)        \     (Y/Δ)
+ *     |       |                 |                      \
+ *      '.___.' '.               |          Rear Left    \   Rear Right
+ *               '.             /             Wheel       \    Wheel
+ *                \.          .'              (A/X)        \   (B/O)
+ *                  \________/
+ *
+ * Uncomment the @Disabled tag below to use this opmode.
+ */
+
+@Disabled
+@Config
+@TeleOp(group = "drive")
+public class MotorDirectionDebugger extends LinearOpMode {
+    public static double MOTOR_POWER = 0.7;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        telemetry.addLine("Press play to begin the debugging opmode");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+
+        while (!isStopRequested()) {
+            telemetry.addLine("Press each button to turn on its respective motor");
+            telemetry.addLine();
+            telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
+            telemetry.addLine();
+
+            if(gamepad1.x) {
+                drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
+                telemetry.addLine("Running Motor: Front Left");
+            } else if(gamepad1.y) {
+                drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
+                telemetry.addLine("Running Motor: Front Right");
+            } else if(gamepad1.b) {
+                drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
+                telemetry.addLine("Running Motor: Rear Right");
+            } else if(gamepad1.a) {
+                drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
+                telemetry.addLine("Running Motor: Rear Left");
+            } else {
+                drive.setMotorPowers(0, 0, 0, 0);
+                telemetry.addLine("Running Motor: None");
+            }
+
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/SplineTest.java

@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/*
+ * This is an example of a more complex path to really test the tuning.
+ */
+@Disabled
+@Autonomous(group = "drive")
+public class SplineTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        Trajectory traj = drive.trajectoryBuilder(new Pose2d())
+                .splineTo(new Vector2d(30, 30), 0)
+                .build();
+
+        drive.followTrajectory(traj);
+
+        sleep(2000);
+
+        drive.followTrajectory(
+                drive.trajectoryBuilder(traj.end(), true)
+                        .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+                        .build()
+        );
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StrafeTest.java

@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class StrafeTest extends LinearOpMode {
+    public static double DISTANCE = 60; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+                .strafeRight(DISTANCE)
+                .build();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.followTrajectory(trajectory);
+
+        Pose2d poseEstimate = drive.getPoseEstimate();
+        telemetry.addData("finalX", poseEstimate.getX());
+        telemetry.addData("finalY", poseEstimate.getY());
+        telemetry.addData("finalHeading", poseEstimate.getHeading());
+        telemetry.update();
+
+        while (!isStopRequested() && opModeIsActive()) ;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StraightTest.java

@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class StraightTest extends LinearOpMode {
+    public static double DISTANCE = 60; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+                .forward(DISTANCE)
+                .build();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.followTrajectory(trajectory);
+
+        Pose2d poseEstimate = drive.getPoseEstimate();
+        telemetry.addData("finalX", poseEstimate.getX());
+        telemetry.addData("finalY", poseEstimate.getY());
+        telemetry.addData("finalHeading", poseEstimate.getHeading());
+        telemetry.update();
+
+        while (!isStopRequested() && opModeIsActive()) ;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackWidthTuner.java

@@ -0,0 +1,90 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/*
+ * This routine determines the effective track width. The procedure works by executing a point turn
+ * with a given angle and measuring the difference between that angle and the actual angle (as
+ * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
+ * given angle / actual angle gives a multiplicative adjustment to the estimated track width
+ * (effective track width = estimated track width * given angle / actual angle). The routine repeats
+ * this procedure a few times and averages the values for additional accuracy. Note: a relatively
+ * accurate track width estimate is important or else the angular constraints will be thrown off.
+ */
+@Config
+@Disabled
+@Autonomous(group = "drive")
+public class TrackWidthTuner extends LinearOpMode {
+    public static double ANGLE = 180; // deg
+    public static int NUM_TRIALS = 5;
+    public static int DELAY = 1000; // ms
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+        // TODO: if you haven't already, set the localizer to something that doesn't depend on
+        // drive encoders for computing the heading
+
+        telemetry.addLine("Press play to begin the track width tuner routine");
+        telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
+        for (int i = 0; i < NUM_TRIALS; i++) {
+            drive.setPoseEstimate(new Pose2d());
+
+            // it is important to handle heading wraparounds
+            double headingAccumulator = 0;
+            double lastHeading = 0;
+
+            drive.turnAsync(Math.toRadians(ANGLE));
+
+            while (!isStopRequested() && drive.isBusy()) {
+                double heading = drive.getPoseEstimate().getHeading();
+                headingAccumulator += Angle.normDelta(heading - lastHeading);
+                lastHeading = heading;
+
+                drive.update();
+            }
+
+            double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
+            trackWidthStats.add(trackWidth);
+
+            sleep(DELAY);
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Tuning complete");
+        telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
+                trackWidthStats.getMean(),
+                trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+        telemetry.update();
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java

@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * This routine determines the effective forward offset for the lateral tracking wheel.
+ * The procedure executes a point turn at a given angle for a certain number of trials,
+ * along with a specified delay in milliseconds. The purpose of this is to track the
+ * change in the y position during the turn. The offset, or distance, of the lateral tracking
+ * wheel from the center or rotation allows the wheel to spin during a point turn, leading
+ * to an incorrect measurement for the y position. This creates an arc around around
+ * the center of rotation with an arc length of change in y and a radius equal to the forward
+ * offset. We can compute this offset by calculating (change in y position) / (change in heading)
+ * which returns the radius if the angle (change in heading) is in radians. This is based
+ * on the arc length formula of length = theta * radius.
+ *
+ * To run this routine, simply adjust the desired angle and specify the number of trials
+ * and the desired delay. Then, run the procedure. Once it finishes, it will print the
+ * average of all the calculated forward offsets derived from the calculation. This calculated
+ * forward offset is then added onto the current forward offset to produce an overall estimate
+ * for the forward offset. You can run this procedure as many times as necessary until a
+ * satisfactory result is produced.
+ */
+@Config
+@Disabled
+@Autonomous(group="drive")
+public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
+    public static double ANGLE = 180; // deg
+    public static int NUM_TRIALS = 5;
+    public static int DELAY = 1000; // ms
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+        }
+
+        telemetry.addLine("Press play to begin the forward offset tuner");
+        telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
+        for (int i = 0; i < NUM_TRIALS; i++) {
+            drive.setPoseEstimate(new Pose2d());
+
+            // it is important to handle heading wraparounds
+            double headingAccumulator = 0;
+            double lastHeading = 0;
+
+            drive.turnAsync(Math.toRadians(ANGLE));
+
+            while (!isStopRequested() && drive.isBusy()) {
+                double heading = drive.getPoseEstimate().getHeading();
+                headingAccumulator += Angle.norm(heading - lastHeading);
+                lastHeading = heading;
+
+                drive.update();
+            }
+
+            double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
+                    drive.getPoseEstimate().getY() / headingAccumulator;
+            forwardOffsetStats.add(forwardOffset);
+
+            sleep(DELAY);
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Tuning complete");
+        telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
+                forwardOffsetStats.getMean(),
+                forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+        telemetry.update();
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java

@@ -0,0 +1,132 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
+ * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
+ * wheels.
+ *
+ * Tuning Routine:
+ *
+ * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
+ * measured value. This need only be an estimated value as you will be tuning it anyways.
+ *
+ * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
+ * similar mark right below the indicator on your bot. This will be your reference point to
+ * ensure you've turned exactly 360°.
+ *
+ * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
+ * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
+ * if you are using the Control Hub.
+ * Ensure the field is showing (select the field view in top right of the page).
+ *
+ * 4. Press play to begin the tuning routine.
+ *
+ * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
+ *
+ * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
+ *
+ * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
+ * on the bot and on the ground you created earlier should be lined up.
+ *
+ * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
+ * StandardTrackingWheelLocalizer.java class.
+ *
+ * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
+ * yourself. Read the heading output and follow the advice stated in the note below to manually
+ * nudge the values yourself.
+ *
+ * Note:
+ * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
+ * a line from the circumference to the center should be present, representing the bot. The line
+ * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
+ * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
+ * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
+ * actual bot, the LATERAL_DISTANCE should be increased.
+ *
+ * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
+ * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
+ * effective center of rotation. You can ignore this effect. The center of rotation will be offset
+ * slightly but your heading will still be fine. This does not affect your overall tracking
+ * precision. The heading should still line up.
+ */
+@Config
+@Disabled
+@TeleOp(group = "drive")
+public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
+    public static int NUM_TURNS = 10;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+        }
+
+        telemetry.addLine("Prior to beginning the routine, please read the directions "
+                + "located in the comments of the opmode file.");
+        telemetry.addLine("Press play to begin the tuning routine.");
+        telemetry.addLine("");
+        telemetry.addLine("Press Y/△ to stop the routine.");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.update();
+
+        double headingAccumulator = 0;
+        double lastHeading = 0;
+
+        boolean tuningFinished = false;
+
+        while (!isStopRequested() && !tuningFinished) {
+            Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
+            drive.setDrivePower(vel);
+
+            drive.update();
+
+            double heading = drive.getPoseEstimate().getHeading();
+            double deltaHeading = heading - lastHeading;
+
+            headingAccumulator += Angle.normDelta(deltaHeading);
+            lastHeading = heading;
+
+            telemetry.clearAll();
+            telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
+            telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
+            telemetry.addLine();
+            telemetry.addLine("Press Y/△ to conclude routine");
+            telemetry.update();
+
+            if (gamepad1.y)
+                tuningFinished = true;
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
+        telemetry.addLine("Effective LATERAL_DISTANCE: " +
+                (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
+
+        telemetry.update();
+
+        while (!isStopRequested()) idle();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java

@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+/*
+ * This is a simple routine to test turning capabilities.
+ */
+@Config
+@Autonomous(group = "drive")
+public class TurnTest extends LinearOpMode {
+    public static double ANGLE = 90; // deg
+    public static double DISTANCE = 24;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        MecanumDrive drive = new MecanumDrive(hardwareMap);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        GamepadEx controller = new GamepadEx(this.gamepad1);
+        boolean zig = false;
+        while (opModeIsActive() && !isStopRequested()) {
+            controller.readButtons();
+            if (controller.wasJustPressed(GamepadKeys.Button.A)) {
+                drive.turn(Math.toRadians(ANGLE));
+            }
+
+            if (controller.wasJustPressed(GamepadKeys.Button.B)) {
+                TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate());
+                builder.forward(DISTANCE);
+                drive.followTrajectory(builder.build());
+            }
+            if (controller.wasJustPressed(GamepadKeys.Button.Y)) {
+                TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate());
+                builder.back(DISTANCE);
+                drive.followTrajectory(builder.build());
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/EmptySequenceException.java

@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence;
+
+
+public class EmptySequenceException extends RuntimeException { }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequence.java

@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.SequenceSegment;
+
+import java.util.Collections;
+import java.util.List;
+
+public class TrajectorySequence {
+    private final List<SequenceSegment> sequenceList;
+
+    public TrajectorySequence(List<SequenceSegment> sequenceList) {
+        if (sequenceList.size() == 0) throw new EmptySequenceException();
+
+        this.sequenceList = Collections.unmodifiableList(sequenceList);
+    }
+
+    public Pose2d start() {
+        return sequenceList.get(0).getStartPose();
+    }
+
+    public Pose2d end() {
+        return sequenceList.get(sequenceList.size() - 1).getEndPose();
+    }
+
+    public double duration() {
+        double total = 0.0;
+
+        for (SequenceSegment segment : sequenceList) {
+            total += segment.getDuration();
+        }
+
+        return total;
+    }
+
+    public SequenceSegment get(int i) {
+        return sequenceList.get(i);
+    }
+
+    public int size() {
+        return sequenceList.size();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceBuilder.java

@@ -0,0 +1,711 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
+import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
+import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
+import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
+import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
+import com.acmerobotics.roadrunner.trajectory.TimeProducer;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.WaitSegment;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
+
+public class TrajectorySequenceBuilder {
+    private final double resolution = 0.25;
+
+    private final TrajectoryVelocityConstraint baseVelConstraint;
+    private final TrajectoryAccelerationConstraint baseAccelConstraint;
+
+    private TrajectoryVelocityConstraint currentVelConstraint;
+    private TrajectoryAccelerationConstraint currentAccelConstraint;
+
+    private final double baseTurnConstraintMaxAngVel;
+    private final double baseTurnConstraintMaxAngAccel;
+
+    private double currentTurnConstraintMaxAngVel;
+    private double currentTurnConstraintMaxAngAccel;
+
+    private final List<SequenceSegment> sequenceSegments;
+
+    private final List<TemporalMarker> temporalMarkers;
+    private final List<DisplacementMarker> displacementMarkers;
+    private final List<SpatialMarker> spatialMarkers;
+
+    private Pose2d lastPose;
+
+    private double tangentOffset;
+
+    private boolean setAbsoluteTangent;
+    private double absoluteTangent;
+
+    private TrajectoryBuilder currentTrajectoryBuilder;
+
+    private double currentDuration;
+    private double currentDisplacement;
+
+    private double lastDurationTraj;
+    private double lastDisplacementTraj;
+
+    public TrajectorySequenceBuilder(
+            Pose2d startPose,
+            Double startTangent,
+            TrajectoryVelocityConstraint baseVelConstraint,
+            TrajectoryAccelerationConstraint baseAccelConstraint,
+            double baseTurnConstraintMaxAngVel,
+            double baseTurnConstraintMaxAngAccel
+    ) {
+        this.baseVelConstraint = baseVelConstraint;
+        this.baseAccelConstraint = baseAccelConstraint;
+
+        this.currentVelConstraint = baseVelConstraint;
+        this.currentAccelConstraint = baseAccelConstraint;
+
+        this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+        this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+        this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+        this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+        sequenceSegments = new ArrayList<>();
+
+        temporalMarkers = new ArrayList<>();
+        displacementMarkers = new ArrayList<>();
+        spatialMarkers = new ArrayList<>();
+
+        lastPose = startPose;
+
+        tangentOffset = 0.0;
+
+        setAbsoluteTangent = (startTangent != null);
+        absoluteTangent = startTangent != null ? startTangent : 0.0;
+
+        currentTrajectoryBuilder = null;
+
+        currentDuration = 0.0;
+        currentDisplacement = 0.0;
+
+        lastDurationTraj = 0.0;
+        lastDisplacementTraj = 0.0;
+    }
+
+    public TrajectorySequenceBuilder(
+            Pose2d startPose,
+            TrajectoryVelocityConstraint baseVelConstraint,
+            TrajectoryAccelerationConstraint baseAccelConstraint,
+            double baseTurnConstraintMaxAngVel,
+            double baseTurnConstraintMaxAngAccel
+    ) {
+        this(
+                startPose, null,
+                baseVelConstraint, baseAccelConstraint,
+                baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
+        );
+    }
+
+    public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
+        return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineTo(
+            Vector2d endPosition,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
+        return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToConstantHeading(
+            Vector2d endPosition,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
+        return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToLinearHeading(
+            Pose2d endPose,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
+        return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder lineToSplineHeading(
+            Pose2d endPose,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
+        return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeTo(
+            Vector2d endPosition,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder forward(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder forward(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder back(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder back(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeLeft(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeLeft(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeRight(double distance) {
+        return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder strafeRight(
+            double distance,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineTo(
+            Vector2d endPosition,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToConstantHeading(
+            Vector2d endPosition,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToLinearHeading(
+            Pose2d endPose,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
+        return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+    }
+
+    public TrajectorySequenceBuilder splineToSplineHeading(
+            Pose2d endPose,
+            double endHeading,
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
+    }
+
+    private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
+        if (currentTrajectoryBuilder == null) newPath();
+
+        try {
+            callback.run();
+        } catch (PathContinuityViolationException e) {
+            newPath();
+            callback.run();
+        }
+
+        Trajectory builtTraj = currentTrajectoryBuilder.build();
+
+        double durationDifference = builtTraj.duration() - lastDurationTraj;
+        double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
+
+        lastPose = builtTraj.end();
+        currentDuration += durationDifference;
+        currentDisplacement += displacementDifference;
+
+        lastDurationTraj = builtTraj.duration();
+        lastDisplacementTraj = builtTraj.getPath().length();
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setTangent(double tangent) {
+        setAbsoluteTangent = true;
+        absoluteTangent = tangent;
+
+        pushPath();
+
+        return this;
+    }
+
+    private TrajectorySequenceBuilder setTangentOffset(double offset) {
+        setAbsoluteTangent = false;
+
+        this.tangentOffset = offset;
+        this.pushPath();
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setReversed(boolean reversed) {
+        return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
+    }
+
+    public TrajectorySequenceBuilder setConstraints(
+            TrajectoryVelocityConstraint velConstraint,
+            TrajectoryAccelerationConstraint accelConstraint
+    ) {
+        this.currentVelConstraint = velConstraint;
+        this.currentAccelConstraint = accelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetConstraints() {
+        this.currentVelConstraint = this.baseVelConstraint;
+        this.currentAccelConstraint = this.baseAccelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
+        this.currentVelConstraint = velConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetVelConstraint() {
+        this.currentVelConstraint = this.baseVelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
+        this.currentAccelConstraint = accelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetAccelConstraint() {
+        this.currentAccelConstraint = this.baseAccelConstraint;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
+        this.currentTurnConstraintMaxAngVel = maxAngVel;
+        this.currentTurnConstraintMaxAngAccel = maxAngAccel;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder resetTurnConstraint() {
+        this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+        this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
+        return this.addTemporalMarker(currentDuration, callback);
+    }
+
+    public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
+        return this.addTemporalMarker(currentDuration + offset, callback);
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
+        return this.addTemporalMarker(0.0, time, callback);
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
+        return this.addTemporalMarker(time -> scale * time + offset, callback);
+    }
+
+    public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
+        this.temporalMarkers.add(new TemporalMarker(time, callback));
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
+        this.spatialMarkers.add(new SpatialMarker(point, callback));
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
+        return this.addDisplacementMarker(currentDisplacement, callback);
+    }
+
+    public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
+        return this.addDisplacementMarker(currentDisplacement + offset, callback);
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
+        return this.addDisplacementMarker(0.0, displacement, callback);
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
+        return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
+    }
+
+    public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
+        displacementMarkers.add(new DisplacementMarker(displacement, callback));
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder turn(double angle) {
+        return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
+    }
+
+    public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
+        pushPath();
+
+        MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
+                new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
+                new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
+                maxAngVel,
+                maxAngAccel
+        );
+
+        sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
+
+        lastPose = new Pose2d(
+                lastPose.getX(), lastPose.getY(),
+                Angle.norm(lastPose.getHeading() + angle)
+        );
+
+        currentDuration += turnProfile.duration();
+
+        return this;
+    }
+
+    public TrajectorySequenceBuilder waitSeconds(double seconds) {
+        pushPath();
+        sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
+
+        currentDuration += seconds;
+        return this;
+    }
+
+    public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
+        pushPath();
+
+        sequenceSegments.add(new TrajectorySegment(trajectory));
+        return this;
+    }
+
+    private void pushPath() {
+        if (currentTrajectoryBuilder != null) {
+            Trajectory builtTraj = currentTrajectoryBuilder.build();
+            sequenceSegments.add(new TrajectorySegment(builtTraj));
+        }
+
+        currentTrajectoryBuilder = null;
+    }
+
+    private void newPath() {
+        if (currentTrajectoryBuilder != null)
+            pushPath();
+
+        lastDurationTraj = 0.0;
+        lastDisplacementTraj = 0.0;
+
+        double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
+
+        currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
+    }
+
+    public TrajectorySequence build() {
+        pushPath();
+
+        List<TrajectoryMarker> globalMarkers = convertMarkersToGlobal(
+                sequenceSegments,
+                temporalMarkers, displacementMarkers, spatialMarkers
+        );
+
+        return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
+    }
+
+    private List<TrajectoryMarker> convertMarkersToGlobal(
+            List<SequenceSegment> sequenceSegments,
+            List<TemporalMarker> temporalMarkers,
+            List<DisplacementMarker> displacementMarkers,
+            List<SpatialMarker> spatialMarkers
+    ) {
+        ArrayList<TrajectoryMarker> trajectoryMarkers = new ArrayList<>();
+
+        // Convert temporal markers
+        for (TemporalMarker marker : temporalMarkers) {
+            trajectoryMarkers.add(
+                    new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
+            );
+        }
+
+        // Convert displacement markers
+        for (DisplacementMarker marker : displacementMarkers) {
+            double time = displacementToTime(
+                    sequenceSegments,
+                    marker.getProducer().produce(currentDisplacement)
+            );
+
+            trajectoryMarkers.add(
+                    new TrajectoryMarker(
+                            time,
+                            marker.getCallback()
+                    )
+            );
+        }
+
+        // Convert spatial markers
+        for (SpatialMarker marker : spatialMarkers) {
+            trajectoryMarkers.add(
+                    new TrajectoryMarker(
+                            pointToTime(sequenceSegments, marker.getPoint()),
+                            marker.getCallback()
+                    )
+            );
+        }
+
+        return trajectoryMarkers;
+    }
+
+    private List<SequenceSegment> projectGlobalMarkersToLocalSegments(List<TrajectoryMarker> markers, List<SequenceSegment> sequenceSegments) {
+        if (sequenceSegments.isEmpty()) return Collections.emptyList();
+
+        markers.sort(Comparator.comparingDouble(TrajectoryMarker::getTime));
+
+        int segmentIndex = 0;
+        double currentTime = 0;
+
+        for (TrajectoryMarker marker : markers) {
+            SequenceSegment segment = null;
+
+            double markerTime = marker.getTime();
+            double segmentOffsetTime = 0;
+
+            while (segmentIndex < sequenceSegments.size()) {
+                SequenceSegment seg = sequenceSegments.get(segmentIndex);
+
+                if (currentTime + seg.getDuration() >= markerTime) {
+                    segment = seg;
+                    segmentOffsetTime = markerTime - currentTime;
+                    break;
+                } else {
+                    currentTime += seg.getDuration();
+                    segmentIndex++;
+                }
+            }
+            if (segmentIndex >= sequenceSegments.size()) {
+                segment = sequenceSegments.get(sequenceSegments.size()-1);
+                segmentOffsetTime = segment.getDuration();
+            }
+
+            SequenceSegment newSegment = null;
+
+            if (segment instanceof WaitSegment) {
+                WaitSegment thisSegment = (WaitSegment) segment;
+                
+                List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getMarkers());
+                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+                newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers);
+            } else if (segment instanceof TurnSegment) {
+                TurnSegment thisSegment = (TurnSegment) segment;
+                
+                List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getMarkers());
+                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+                newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers);
+            } else if (segment instanceof TrajectorySegment) {
+                TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+                List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
+                newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
+
+                newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers));
+            }
+
+            sequenceSegments.set(segmentIndex, newSegment);
+        }
+
+        return sequenceSegments;
+    }
+
+    // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
+    // note: this assumes that the profile position is monotonic increasing
+    private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
+        double tLo = 0.0;
+        double tHi = profile.duration();
+        while (!(Math.abs(tLo - tHi) < 1e-6)) {
+            double tMid = 0.5 * (tLo + tHi);
+            if (profile.get(tMid).getX() > s) {
+                tHi = tMid;
+            } else {
+                tLo = tMid;
+            }
+        }
+        return 0.5 * (tLo + tHi);
+    }
+
+    private Double displacementToTime(List<SequenceSegment> sequenceSegments, double s) {
+        double currentTime = 0.0;
+        double currentDisplacement = 0.0;
+
+        for (SequenceSegment segment : sequenceSegments) {
+            if (segment instanceof TrajectorySegment) {
+                TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+                double segmentLength = thisSegment.getTrajectory().getPath().length();
+
+                if (currentDisplacement + segmentLength > s) {
+                    double target = s - currentDisplacement;
+                    double timeInSegment = motionProfileDisplacementToTime(
+                            thisSegment.getTrajectory().getProfile(),
+                            target
+                    );
+
+                    return currentTime + timeInSegment;
+                } else {
+                    currentDisplacement += segmentLength;
+                    currentTime += thisSegment.getTrajectory().duration();
+                }
+            } else {
+                currentTime += segment.getDuration();
+            }
+        }
+
+        return 0.0;
+    }
+
+    private Double pointToTime(List<SequenceSegment> sequenceSegments, Vector2d point) {
+        class ComparingPoints {
+            private final double distanceToPoint;
+            private final double totalDisplacement;
+            private final double thisPathDisplacement;
+
+            public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
+                this.distanceToPoint = distanceToPoint;
+                this.totalDisplacement = totalDisplacement;
+                this.thisPathDisplacement = thisPathDisplacement;
+            }
+        }
+
+        List<ComparingPoints> projectedPoints = new ArrayList<>();
+
+        for (SequenceSegment segment : sequenceSegments) {
+            if (segment instanceof TrajectorySegment) {
+                TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+                double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
+                Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
+                double distanceToPoint = point.minus(projectedPoint).norm();
+
+                double totalDisplacement = 0.0;
+
+                for (ComparingPoints comparingPoint : projectedPoints) {
+                    totalDisplacement += comparingPoint.totalDisplacement;
+                }
+
+                totalDisplacement += displacement;
+
+                projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
+            }
+        }
+
+        ComparingPoints closestPoint = null;
+
+        for (ComparingPoints comparingPoint : projectedPoints) {
+            if (closestPoint == null) {
+                closestPoint = comparingPoint;
+                continue;
+            }
+
+            if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
+                closestPoint = comparingPoint;
+        }
+
+        return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
+    }
+
+    private interface AddPathCallback {
+        void run();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java

@@ -0,0 +1,307 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment.WaitSegment;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.DashboardUtil;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LogFiles;
+
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public class TrajectorySequenceRunner {
+    public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
+    public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
+    public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
+
+    public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
+    public static String COLOR_ACTIVE_TURN = "#7c4dff";
+    public static String COLOR_ACTIVE_WAIT = "#dd2c00";
+
+    public static int POSE_HISTORY_LIMIT = 100;
+
+    private final TrajectoryFollower follower;
+
+    private final PIDFController turnController;
+
+    private final NanoClock clock;
+
+    private TrajectorySequence currentTrajectorySequence;
+    private double currentSegmentStartTime;
+    private int currentSegmentIndex;
+    private int lastSegmentIndex;
+
+    private Pose2d lastPoseError = new Pose2d();
+
+    List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
+
+    private final FtcDashboard dashboard;
+    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
+
+    private VoltageSensor voltageSensor;
+
+    private List<Integer> lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels;
+
+    public TrajectorySequenceRunner(
+            TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients, VoltageSensor voltageSensor,
+            List<Integer> lastDriveEncPositions, List<Integer> lastDriveEncVels, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels
+    ) {
+        this.follower = follower;
+
+        turnController = new PIDFController(headingPIDCoefficients);
+        turnController.setInputBounds(0, 2 * Math.PI);
+
+        this.voltageSensor = voltageSensor;
+
+        this.lastDriveEncPositions = lastDriveEncPositions;
+        this.lastDriveEncVels = lastDriveEncVels;
+        this.lastTrackingEncPositions = lastTrackingEncPositions;
+        this.lastTrackingEncVels = lastTrackingEncVels;
+
+        clock = NanoClock.system();
+
+        dashboard = FtcDashboard.getInstance();
+        dashboard.setTelemetryTransmissionInterval(25);
+    }
+
+    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+        currentTrajectorySequence = trajectorySequence;
+        currentSegmentStartTime = clock.seconds();
+        currentSegmentIndex = 0;
+        lastSegmentIndex = -1;
+    }
+
+    public @Nullable
+    DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
+        Pose2d targetPose = null;
+        DriveSignal driveSignal = null;
+
+        TelemetryPacket packet = new TelemetryPacket();
+        Canvas fieldOverlay = packet.fieldOverlay();
+
+        SequenceSegment currentSegment = null;
+
+        if (currentTrajectorySequence != null) {
+            if (currentSegmentIndex >= currentTrajectorySequence.size()) {
+                for (TrajectoryMarker marker : remainingMarkers) {
+                    marker.getCallback().onMarkerReached();
+                }
+
+                remainingMarkers.clear();
+
+                currentTrajectorySequence = null;
+            }
+
+            if (currentTrajectorySequence == null)
+                return new DriveSignal();
+
+            double now = clock.seconds();
+            boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
+
+            currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
+
+            if (isNewTransition) {
+                currentSegmentStartTime = now;
+                lastSegmentIndex = currentSegmentIndex;
+
+                for (TrajectoryMarker marker : remainingMarkers) {
+                    marker.getCallback().onMarkerReached();
+                }
+
+                remainingMarkers.clear();
+
+                remainingMarkers.addAll(currentSegment.getMarkers());
+                Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
+            }
+
+            double deltaTime = now - currentSegmentStartTime;
+
+            if (currentSegment instanceof TrajectorySegment) {
+                Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+                if (isNewTransition)
+                    follower.followTrajectory(currentTrajectory);
+
+                if (!follower.isFollowing()) {
+                    currentSegmentIndex++;
+
+                    driveSignal = new DriveSignal();
+                } else {
+                    driveSignal = follower.update(poseEstimate, poseVelocity);
+                    lastPoseError = follower.getLastError();
+                }
+
+                targetPose = currentTrajectory.get(deltaTime);
+            } else if (currentSegment instanceof TurnSegment) {
+                MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
+
+                turnController.setTargetPosition(targetState.getX());
+
+                double correction = turnController.update(poseEstimate.getHeading());
+
+                double targetOmega = targetState.getV();
+                double targetAlpha = targetState.getA();
+
+                lastPoseError = new Pose2d(0, 0, turnController.getLastError());
+
+                Pose2d startPose = currentSegment.getStartPose();
+                targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
+
+                driveSignal = new DriveSignal(
+                        new Pose2d(0, 0, targetOmega + correction),
+                        new Pose2d(0, 0, targetAlpha)
+                );
+
+                if (deltaTime >= currentSegment.getDuration()) {
+                    currentSegmentIndex++;
+                    driveSignal = new DriveSignal();
+                }
+            } else if (currentSegment instanceof WaitSegment) {
+                lastPoseError = new Pose2d();
+
+                targetPose = currentSegment.getStartPose();
+                driveSignal = new DriveSignal();
+
+                if (deltaTime >= currentSegment.getDuration()) {
+                    currentSegmentIndex++;
+                }
+            }
+
+            while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
+                remainingMarkers.get(0).getCallback().onMarkerReached();
+                remainingMarkers.remove(0);
+            }
+        }
+
+        poseHistory.add(poseEstimate);
+
+        if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
+            poseHistory.removeFirst();
+        }
+
+        final double NOMINAL_VOLTAGE = 12.0;
+        double voltage = voltageSensor.getVoltage();
+        if (driveSignal != null && !DriveConstants.RUN_USING_ENCODER) {
+            driveSignal = new DriveSignal(
+                    driveSignal.getVel().times(NOMINAL_VOLTAGE / voltage),
+                    driveSignal.getAccel().times(NOMINAL_VOLTAGE / voltage)
+            );
+        }
+
+        if (targetPose != null) {
+            LogFiles.record(
+                    targetPose, poseEstimate, voltage,
+                    lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels
+            );
+        }
+
+        packet.put("x", poseEstimate.getX());
+        packet.put("y", poseEstimate.getY());
+        packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
+
+        packet.put("xError", getLastPoseError().getX());
+        packet.put("yError", getLastPoseError().getY());
+        packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
+
+        draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
+
+        dashboard.sendTelemetryPacket(packet);
+
+        return driveSignal;
+    }
+
+    private void draw(
+            Canvas fieldOverlay,
+            TrajectorySequence sequence, SequenceSegment currentSegment,
+            Pose2d targetPose, Pose2d poseEstimate
+    ) {
+        if (sequence != null) {
+            for (int i = 0; i < sequence.size(); i++) {
+                SequenceSegment segment = sequence.get(i);
+
+                if (segment instanceof TrajectorySegment) {
+                    fieldOverlay.setStrokeWidth(1);
+                    fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
+
+                    DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
+                } else if (segment instanceof TurnSegment) {
+                    Pose2d pose = segment.getStartPose();
+
+                    fieldOverlay.setFill(COLOR_INACTIVE_TURN);
+                    fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
+                } else if (segment instanceof WaitSegment) {
+                    Pose2d pose = segment.getStartPose();
+
+                    fieldOverlay.setStrokeWidth(1);
+                    fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
+                    fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+                }
+            }
+        }
+
+        if (currentSegment != null) {
+            if (currentSegment instanceof TrajectorySegment) {
+                Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+                fieldOverlay.setStrokeWidth(1);
+                fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
+
+                DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
+            } else if (currentSegment instanceof TurnSegment) {
+                Pose2d pose = currentSegment.getStartPose();
+
+                fieldOverlay.setFill(COLOR_ACTIVE_TURN);
+                fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
+            } else if (currentSegment instanceof WaitSegment) {
+                Pose2d pose = currentSegment.getStartPose();
+
+                fieldOverlay.setStrokeWidth(1);
+                fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
+                fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+            }
+        }
+
+        if (targetPose != null) {
+            fieldOverlay.setStrokeWidth(1);
+            fieldOverlay.setStroke("#4CAF50");
+            DashboardUtil.drawRobot(fieldOverlay, targetPose);
+        }
+
+        fieldOverlay.setStroke("#3F51B5");
+        DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
+
+        fieldOverlay.setStroke("#3F51B5");
+        DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
+    }
+
+    public Pose2d getLastPoseError() {
+        return lastPoseError;
+    }
+
+    public boolean isBusy() {
+        return currentTrajectorySequence != null;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.java

@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public abstract class SequenceSegment {
+    private final double duration;
+    private final Pose2d startPose;
+    private final Pose2d endPose;
+    private final List<TrajectoryMarker> markers;
+
+    protected SequenceSegment(
+            double duration,
+            Pose2d startPose, Pose2d endPose,
+            List<TrajectoryMarker> markers
+    ) {
+        this.duration = duration;
+        this.startPose = startPose;
+        this.endPose = endPose;
+        this.markers = markers;
+    }
+
+    public double getDuration() {
+        return this.duration;
+    }
+
+    public Pose2d getStartPose() {
+        return startPose;
+    }
+
+    public Pose2d getEndPose() {
+        return endPose;
+    }
+
+    public List<TrajectoryMarker> getMarkers() {
+        return markers;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.java

@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+
+import java.util.Collections;
+
+public final class TrajectorySegment extends SequenceSegment {
+    private final Trajectory trajectory;
+
+    public TrajectorySegment(Trajectory trajectory) {
+        // Note: Markers are already stored in the `Trajectory` itself.
+        // This class should not hold any markers
+        super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
+        this.trajectory = trajectory;
+    }
+
+    public Trajectory getTrajectory() {
+        return this.trajectory;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/sequencesegment/TurnSegment.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import java.util.List;
+
+public final class TurnSegment extends SequenceSegment {
+    private final double totalRotation;
+    private final MotionProfile motionProfile;
+
+    public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List<TrajectoryMarker> markers) {
+        super(
+                motionProfile.duration(),
+                startPose,
+                new Pose2d(
+                        startPose.getX(), startPose.getY(),
+                        Angle.norm(startPose.getHeading() + totalRotation)
+                ),
+                markers
+        );
+
+        this.totalRotation = totalRotation;
+        this.motionProfile = motionProfile;
+    }
+
+    public final double getTotalRotation() {
+        return this.totalRotation;
+    }
+
+    public final MotionProfile getMotionProfile() {
+        return this.motionProfile;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/sequencesegment/WaitSegment.java

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public final class WaitSegment extends SequenceSegment {
+    public WaitSegment(Pose2d pose, double seconds, List<TrajectoryMarker> markers) {
+        super(seconds, pose, pose, markers);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/AssetsTrajectoryManager.java

@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.IOException;
+import java.io.InputStream;
+
+/**
+ * Set of utilities for loading trajectories from assets (the plugin save location).
+ */
+public class AssetsTrajectoryManager {
+
+    /**
+     * Loads the group config.
+     */
+    public static @Nullable
+    TrajectoryGroupConfig loadGroupConfig() {
+        try {
+            InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+                    "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
+            return TrajectoryConfigManager.loadGroupConfig(inputStream);
+        } catch (IOException e) {
+            return null;
+        }
+    }
+
+    /**
+     * Loads a trajectory config with the given name.
+     */
+    public static @Nullable TrajectoryConfig loadConfig(String name) {
+        try {
+            InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+                    "trajectory/" + name + ".yaml");
+            return TrajectoryConfigManager.loadConfig(inputStream);
+        } catch (IOException e) {
+            return null;
+        }
+    }
+
+    /**
+     * Loads a trajectory builder with the given name.
+     */
+    public static @Nullable TrajectoryBuilder loadBuilder(String name) {
+        TrajectoryGroupConfig groupConfig = loadGroupConfig();
+        TrajectoryConfig config = loadConfig(name);
+        if (groupConfig == null || config == null) {
+            return null;
+        }
+        return config.toTrajectoryBuilder(groupConfig);
+    }
+
+    /**
+     * Loads a trajectory with the given name.
+     */
+    public static @Nullable Trajectory load(String name) {
+        TrajectoryBuilder builder = loadBuilder(name);
+        if (builder == null) {
+            return null;
+        }
+        return builder.build();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/AxesSigns.java

@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+/**
+ * IMU axes signs in the order XYZ (after remapping).
+ */
+public enum AxesSigns {
+    PPP(0b000),
+    PPN(0b001),
+    PNP(0b010),
+    PNN(0b011),
+    NPP(0b100),
+    NPN(0b101),
+    NNP(0b110),
+    NNN(0b111);
+
+    public final int bVal;
+
+    AxesSigns(int bVal) {
+        this.bVal = bVal;
+    }
+
+    public static AxesSigns fromBinaryValue(int bVal) {
+        int maskedVal = bVal & 0x07;
+        switch (maskedVal) {
+            case 0b000:
+                return AxesSigns.PPP;
+            case 0b001:
+                return AxesSigns.PPN;
+            case 0b010:
+                return AxesSigns.PNP;
+            case 0b011:
+                return AxesSigns.PNN;
+            case 0b100:
+                return AxesSigns.NPP;
+            case 0b101:
+                return AxesSigns.NPN;
+            case 0b110:
+                return AxesSigns.NNP;
+            case 0b111:
+                return AxesSigns.NNN;
+            default:
+                throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/AxisDirection.java

@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+/**
+ * A direction for an axis to be remapped to
+ */
+public enum AxisDirection {
+    POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/DashboardUtil.java

@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.Path;
+
+import java.util.List;
+
+/**
+ * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
+ */
+public class DashboardUtil {
+    private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
+    private static final double ROBOT_RADIUS = 9; // in
+
+
+    public static void drawPoseHistory(Canvas canvas, List<Pose2d> poseHistory) {
+        double[] xPoints = new double[poseHistory.size()];
+        double[] yPoints = new double[poseHistory.size()];
+        for (int i = 0; i < poseHistory.size(); i++) {
+            Pose2d pose = poseHistory.get(i);
+            xPoints[i] = pose.getX();
+            yPoints[i] = pose.getY();
+        }
+        canvas.strokePolyline(xPoints, yPoints);
+    }
+
+    public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
+        int samples = (int) Math.ceil(path.length() / resolution);
+        double[] xPoints = new double[samples];
+        double[] yPoints = new double[samples];
+        double dx = path.length() / (samples - 1);
+        for (int i = 0; i < samples; i++) {
+            double displacement = i * dx;
+            Pose2d pose = path.get(displacement);
+            xPoints[i] = pose.getX();
+            yPoints[i] = pose.getY();
+        }
+        canvas.strokePolyline(xPoints, yPoints);
+    }
+
+    public static void drawSampledPath(Canvas canvas, Path path) {
+        drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
+    }
+
+    public static void drawRobot(Canvas canvas, Pose2d pose) {
+        canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
+        Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
+        double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
+        double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
+        canvas.strokeLine(x1, y1, x2, y2);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/Encoder.java

@@ -0,0 +1,125 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+/**
+ * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
+ * slot's motor direction
+ */
+public class Encoder {
+    private final static int CPS_STEP = 0x10000;
+
+    private static double inverseOverflow(double input, double estimate) {
+        // convert to uint16
+        int real = (int) input & 0xffff;
+        // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
+        // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
+        real += ((real % 20) / 4) * CPS_STEP;
+        // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
+        real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
+        return real;
+    }
+
+    public enum Direction {
+        FORWARD(1),
+        REVERSE(-1);
+
+        private int multiplier;
+
+        Direction(int multiplier) {
+            this.multiplier = multiplier;
+        }
+
+        public int getMultiplier() {
+            return multiplier;
+        }
+    }
+
+    private DcMotorEx motor;
+    private NanoClock clock;
+
+    private Direction direction;
+
+    private int lastPosition;
+    private int velocityEstimateIdx;
+    private double[] velocityEstimates;
+    private double lastUpdateTime;
+
+    public Encoder(DcMotorEx motor, NanoClock clock) {
+        this.motor = motor;
+        this.clock = clock;
+
+        this.direction = Direction.FORWARD;
+
+        this.lastPosition = 0;
+        this.velocityEstimates = new double[3];
+        this.lastUpdateTime = clock.seconds();
+    }
+
+    public Encoder(DcMotorEx motor) {
+        this(motor, NanoClock.system());
+    }
+
+    public Direction getDirection() {
+        return direction;
+    }
+
+    private int getMultiplier() {
+        return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
+    }
+
+    /**
+     * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
+     * @param direction either reverse or forward depending on if encoder counts should be negated
+     */
+    public void setDirection(Direction direction) {
+        this.direction = direction;
+    }
+
+    /**
+     * Gets the position from the underlying motor and adjusts for the set direction.
+     * Additionally, this method updates the velocity estimates used for compensated velocity
+     *
+     * @return encoder position
+     */
+    public int getCurrentPosition() {
+        int multiplier = getMultiplier();
+        int currentPosition = motor.getCurrentPosition() * multiplier;
+        if (currentPosition != lastPosition) {
+            double currentTime = clock.seconds();
+            double dt = currentTime - lastUpdateTime;
+            velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
+            velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
+            lastPosition = currentPosition;
+            lastUpdateTime = currentTime;
+        }
+        return currentPosition;
+    }
+
+    /**
+     * Gets the velocity directly from the underlying motor and compensates for the direction
+     * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
+     *
+     * @return raw velocity
+     */
+    public double getRawVelocity() {
+        int multiplier = getMultiplier();
+        return motor.getVelocity() * multiplier;
+    }
+
+    /**
+     * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
+     * that are lost in overflow due to velocity being transmitted as 16 bits.
+     * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
+     *
+     * @return corrected velocity
+     */
+    public double getCorrectedVelocity() {
+        double median = velocityEstimates[0] > velocityEstimates[1]
+                ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
+                : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
+        return inverseOverflow(getRawVelocity(), median);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java

@@ -0,0 +1,263 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+import android.annotation.SuppressLint;
+import android.content.Context;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.fasterxml.jackson.core.JsonFactory;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.ObjectWriter;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.WebHandlerManager;
+
+import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer;
+
+
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.text.DateFormat;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Date;
+import java.util.List;
+import java.util.Objects;
+
+import fi.iki.elonen.NanoHTTPD;
+
+public final class LogFiles {
+    private static final File ROOT =
+            new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/");
+
+    public static LogFile log = new LogFile("uninitialized");
+
+    public static class LogFile {
+        public String version = "quickstart1 v2";
+
+        public String opModeName;
+        public long msInit = System.currentTimeMillis();
+        public long nsInit = System.nanoTime();
+        public long nsStart, nsStop;
+
+        public double ticksPerRev = DriveConstants.TICKS_PER_REV;
+        public double maxRpm = DriveConstants.MAX_RPM;
+        public boolean runUsingEncoder = DriveConstants.RUN_USING_ENCODER;
+        public double motorP = DriveConstants.MOTOR_VELO_PID.p;
+        public double motorI = DriveConstants.MOTOR_VELO_PID.i;
+        public double motorD = DriveConstants.MOTOR_VELO_PID.d;
+        public double motorF = DriveConstants.MOTOR_VELO_PID.f;
+        public double wheelRadius = DriveConstants.WHEEL_RADIUS;
+        public double gearRatio = DriveConstants.GEAR_RATIO;
+        public double trackWidth = DriveConstants.TRACK_WIDTH;
+        public double kV = DriveConstants.kV;
+        public double kA = DriveConstants.kA;
+        public double kStatic = DriveConstants.kStatic;
+        public double maxVel = DriveConstants.MAX_VEL;
+        public double maxAccel = DriveConstants.MAX_ACCEL;
+        public double maxAngVel = DriveConstants.MAX_ANG_VEL;
+        public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL;
+
+        public double mecTransP = MecanumDrive.TRANSLATIONAL_PID.kP;
+        public double mecTransI = MecanumDrive.TRANSLATIONAL_PID.kI;
+        public double mecTransD = MecanumDrive.TRANSLATIONAL_PID.kD;
+        public double mecHeadingP = MecanumDrive.HEADING_PID.kP;
+        public double mecHeadingI = MecanumDrive.HEADING_PID.kI;
+        public double mecHeadingD = MecanumDrive.HEADING_PID.kD;
+        public double mecLateralMultiplier = MecanumDrive.LATERAL_MULTIPLIER;
+
+        public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV;
+        public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS;
+        public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;
+        public double trackingLateralDistance = StandardTrackingWheelLocalizer.LATERAL_DISTANCE;
+        public double trackingForwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET;
+
+        public RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = DriveConstants.LOGO_FACING_DIR;
+        public RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = DriveConstants.USB_FACING_DIR;
+
+        public List<Long> nsTimes = new ArrayList<>();
+
+        public List<Double> targetXs = new ArrayList<>();
+        public List<Double> targetYs = new ArrayList<>();
+        public List<Double> targetHeadings = new ArrayList<>();
+
+        public List<Double> xs = new ArrayList<>();
+        public List<Double> ys = new ArrayList<>();
+        public List<Double> headings = new ArrayList<>();
+
+        public List<Double> voltages = new ArrayList<>();
+
+        public List<List<Integer>> driveEncPositions = new ArrayList<>();
+        public List<List<Integer>> driveEncVels = new ArrayList<>();
+        public List<List<Integer>> trackingEncPositions = new ArrayList<>();
+        public List<List<Integer>> trackingEncVels = new ArrayList<>();
+
+        public LogFile(String opModeName) {
+            this.opModeName = opModeName;
+        }
+    }
+
+    public static void record(
+            Pose2d targetPose, Pose2d pose, double voltage,
+            List<Integer> lastDriveEncPositions, List<Integer> lastDriveEncVels, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels
+    ) {
+        long nsTime = System.nanoTime();
+        if (nsTime - log.nsStart > 3 * 60 * 1_000_000_000L) {
+            return;
+        }
+
+        log.nsTimes.add(nsTime);
+
+        log.targetXs.add(targetPose.getX());
+        log.targetYs.add(targetPose.getY());
+        log.targetHeadings.add(targetPose.getHeading());
+
+        log.xs.add(pose.getX());
+        log.ys.add(pose.getY());
+        log.headings.add(pose.getHeading());
+
+        log.voltages.add(voltage);
+
+        while (log.driveEncPositions.size() < lastDriveEncPositions.size()) {
+            log.driveEncPositions.add(new ArrayList<>());
+        }
+        while (log.driveEncVels.size() < lastDriveEncVels.size()) {
+            log.driveEncVels.add(new ArrayList<>());
+        }
+        while (log.trackingEncPositions.size() < lastTrackingEncPositions.size()) {
+            log.trackingEncPositions.add(new ArrayList<>());
+        }
+        while (log.trackingEncVels.size() < lastTrackingEncVels.size()) {
+            log.trackingEncVels.add(new ArrayList<>());
+        }
+
+        for (int i = 0; i < lastDriveEncPositions.size(); i++) {
+            log.driveEncPositions.get(i).add(lastDriveEncPositions.get(i));
+        }
+        for (int i = 0; i < lastDriveEncVels.size(); i++) {
+            log.driveEncVels.get(i).add(lastDriveEncVels.get(i));
+        }
+        for (int i = 0; i < lastTrackingEncPositions.size(); i++) {
+            log.trackingEncPositions.get(i).add(lastTrackingEncPositions.get(i));
+        }
+        for (int i = 0; i < lastTrackingEncVels.size(); i++) {
+            log.trackingEncVels.get(i).add(lastTrackingEncVels.get(i));
+        }
+    }
+
+    private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {
+        @SuppressLint("SimpleDateFormat")
+        final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS");
+
+        final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory())
+                .writerWithDefaultPrettyPrinter();
+
+        @Override
+        public void onOpModePreInit(OpMode opMode) {
+            log = new LogFile(opMode.getClass().getCanonicalName());
+
+            // clean up old files
+            File[] fs = Objects.requireNonNull(ROOT.listFiles());
+            Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified()));
+            long totalSizeBytes = 0;
+            for (File f : fs) {
+                totalSizeBytes += f.length();
+            }
+
+            int i = 0;
+            while (i < fs.length && totalSizeBytes >= 32 * 1000 * 1000) {
+                totalSizeBytes -= fs[i].length();
+                if (!fs[i].delete()) {
+                    RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath());
+                }
+                ++i;
+            }
+        }
+
+        @Override
+        public void onOpModePreStart(OpMode opMode) {
+            log.nsStart = System.nanoTime();
+        }
+
+        @Override
+        public void onOpModePostStop(OpMode opMode) {
+            log.nsStop = System.nanoTime();
+
+            if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) {
+                //noinspection ResultOfMethodCallIgnored
+                ROOT.mkdirs();
+
+                String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json";
+                File file = new File(ROOT, filename);
+                try {
+                    jsonWriter.writeValue(file, log);
+                } catch (IOException e) {
+                    RobotLog.setGlobalErrorMsg(new RuntimeException(e),
+                            "Unable to write data to " + file.getAbsolutePath());
+                }
+            }
+        }
+    };
+
+    @WebHandlerRegistrar
+    public static void registerRoutes(Context context, WebHandlerManager manager) {
+        //noinspection ResultOfMethodCallIgnored
+        ROOT.mkdirs();
+
+        // op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves
+        // don't use @OnCreateEventLoop because it's unreliable
+        OpModeManagerImpl.getOpModeManagerOfActivity(
+                AppUtil.getInstance().getActivity()
+        ).registerListener(notifHandler);
+
+        manager.register("/logs", session -> {
+            final StringBuilder sb = new StringBuilder();
+            sb.append("<!doctype html><html><head><title>Logs</title></head><body><ul>");
+            File[] fs = Objects.requireNonNull(ROOT.listFiles());
+            Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified()));
+            for (File f : fs) {
+                sb.append("<li><a href=\"/logs/download?file=");
+                sb.append(f.getName());
+                sb.append("\" download=\"");
+                sb.append(f.getName());
+                sb.append("\">");
+                sb.append(f.getName());
+                sb.append("</a></li>");
+            }
+            sb.append("</ul></body></html>");
+            return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK,
+                    NanoHTTPD.MIME_HTML, sb.toString());
+        });
+
+        manager.register("/logs/download", session -> {
+            final String[] pairs = session.getQueryParameterString().split("&");
+            if (pairs.length != 1) {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+                        NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length);
+            }
+
+            final String[] parts = pairs[0].split("=");
+            if (!parts[0].equals("file")) {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+                        NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]);
+            }
+
+            File f = new File(ROOT, parts[1]);
+            if (!f.exists()) {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
+                        NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist");
+            }
+
+            return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
+                    "application/json", new FileInputStream(f));
+        });
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LoggingUtil.java

@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * Utility functions for log files.
+ */
+public class LoggingUtil {
+    public static final File ROAD_RUNNER_FOLDER =
+            new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
+
+    private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
+
+    private static void buildLogList(List<File> logFiles, File dir) {
+        for (File file : dir.listFiles()) {
+            if (file.isDirectory()) {
+                buildLogList(logFiles, file);
+            } else {
+                logFiles.add(file);
+            }
+        }
+    }
+
+    private static void pruneLogsIfNecessary() {
+        List<File> logFiles = new ArrayList<>();
+        buildLogList(logFiles, ROAD_RUNNER_FOLDER);
+        Collections.sort(logFiles, (lhs, rhs) ->
+                Long.compare(lhs.lastModified(), rhs.lastModified()));
+
+        long dirSize = 0;
+        for (File file: logFiles) {
+            dirSize += file.length();
+        }
+
+        while (dirSize > LOG_QUOTA) {
+            if (logFiles.size() == 0) break;
+            File fileToRemove = logFiles.remove(0);
+            dirSize -= fileToRemove.length();
+            //noinspection ResultOfMethodCallIgnored
+            fileToRemove.delete();
+        }
+    }
+
+    /**
+     * Obtain a log file with the provided name
+     */
+    public static File getLogFile(String name) {
+        //noinspection ResultOfMethodCallIgnored
+        ROAD_RUNNER_FOLDER.mkdirs();
+
+        pruneLogsIfNecessary();
+
+        return new File(ROAD_RUNNER_FOLDER, name);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LynxModuleUtil.java

@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Collection of utilites for interacting with Lynx modules.
+ */
+public class LynxModuleUtil {
+
+    private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
+
+    /**
+     * Parsed representation of a Lynx module firmware version.
+     */
+    public static class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
+        public final int major;
+        public final int minor;
+        public final int eng;
+
+        public LynxFirmwareVersion(int major, int minor, int eng) {
+            this.major = major;
+            this.minor = minor;
+            this.eng = eng;
+        }
+
+        @Override
+        public boolean equals(Object other) {
+            if (other instanceof LynxFirmwareVersion) {
+                LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
+                return major == otherVersion.major && minor == otherVersion.minor &&
+                        eng == otherVersion.eng;
+            } else {
+                return false;
+            }
+        }
+
+        @Override
+        public int compareTo(LynxFirmwareVersion other) {
+            int majorComp = Integer.compare(major, other.major);
+            if (majorComp == 0) {
+                int minorComp = Integer.compare(minor, other.minor);
+                if (minorComp == 0) {
+                    return Integer.compare(eng, other.eng);
+                } else {
+                    return minorComp;
+                }
+            } else {
+                return majorComp;
+            }
+        }
+
+        @Override
+        public String toString() {
+            return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
+        }
+    }
+
+    /**
+     * Retrieve and parse Lynx module firmware version.
+     * @param module Lynx module
+     * @return parsed firmware version
+     */
+    public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
+        String versionString = module.getNullableFirmwareVersionString();
+        if (versionString == null) {
+            return null;
+        }
+
+        String[] parts = versionString.split("[ :,]+");
+        try {
+            // note: for now, we ignore the hardware entry
+            return new LynxFirmwareVersion(
+                    Integer.parseInt(parts[3]),
+                    Integer.parseInt(parts[5]),
+                    Integer.parseInt(parts[7])
+            );
+        } catch (NumberFormatException e) {
+            return null;
+        }
+    }
+
+    /**
+     * Exception indicating an outdated Lynx firmware version.
+     */
+    public static class LynxFirmwareVersionException extends RuntimeException {
+        public LynxFirmwareVersionException(String detailMessage) {
+            super(detailMessage);
+        }
+    }
+
+    /**
+     * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
+     * @param hardwareMap hardware map containing Lynx modules
+     */
+    public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
+        Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            LynxFirmwareVersion version = getFirmwareVersion(module);
+            if (version == null || version.compareTo(MIN_VERSION) < 0) {
+                for (String name : hardwareMap.getNamesOf(module)) {
+                    outdatedModules.put(name, version);
+                }
+            }
+        }
+        if (outdatedModules.size() > 0) {
+            StringBuilder msgBuilder = new StringBuilder();
+            msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
+            msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
+                    MIN_VERSION.toString()));
+            for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
+                msgBuilder.append(Misc.formatInvariant(
+                        "\t%s: %s\n", entry.getKey(),
+                        entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
+            }
+            throw new LynxFirmwareVersionException(msgBuilder.toString());
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/RegressionUtil.java

@@ -0,0 +1,156 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+
+import org.apache.commons.math3.stat.regression.SimpleRegression;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Various regression utilities.
+ */
+public class RegressionUtil {
+
+    /**
+     * Feedforward parameter estimates from the ramp regression and additional summary statistics
+     */
+    public static class RampResult {
+        public final double kV, kStatic, rSquare;
+
+        public RampResult(double kV, double kStatic, double rSquare) {
+            this.kV = kV;
+            this.kStatic = kStatic;
+            this.rSquare = rSquare;
+        }
+    }
+
+    /**
+     * Feedforward parameter estimates from the ramp regression and additional summary statistics
+     */
+    public static class AccelResult {
+        public final double kA, rSquare;
+
+        public AccelResult(double kA, double rSquare) {
+            this.kA = kA;
+            this.rSquare = rSquare;
+        }
+    }
+
+    /**
+     * Numerically compute dy/dx from the given x and y values. The returned list is padded to match
+     * the length of the original sequences.
+     *
+     * @param x x-values
+     * @param y y-values
+     * @return derivative values
+     */
+    private static List<Double> numericalDerivative(List<Double> x, List<Double> y) {
+        List<Double> deriv = new ArrayList<>(x.size());
+        for (int i = 1; i < x.size() - 1; i++) {
+            deriv.add(
+                    (y.get(i + 1) - y.get(i - 1)) /
+                    (x.get(i + 1) - x.get(i - 1))
+            );
+        }
+        // copy endpoints to pad output
+        deriv.add(0, deriv.get(0));
+        deriv.add(deriv.get(deriv.size() - 1));
+        return deriv;
+    }
+
+    /**
+     * Run regression to compute velocity and static feedforward from ramp test data.
+     *
+     * Here's the general procedure for gathering the requisite data:
+     *   1. Slowly ramp the motor power/voltage and record encoder values along the way.
+     *   2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
+     *      (kV) and an optional intercept (kStatic).
+     *
+     * @param timeSamples time samples
+     * @param positionSamples position samples
+     * @param powerSamples power samples
+     * @param fitStatic fit kStatic
+     * @param file log file
+     */
+    public static RampResult fitRampData(List<Double> timeSamples, List<Double> positionSamples,
+                                         List<Double> powerSamples, boolean fitStatic,
+                                         @Nullable File file) {
+        if (file != null) {
+            try (PrintWriter pw = new PrintWriter(file)) {
+                pw.println("time,position,power");
+                for (int i = 0; i < timeSamples.size(); i++) {
+                    double time = timeSamples.get(i);
+                    double pos = positionSamples.get(i);
+                    double power = powerSamples.get(i);
+                    pw.println(time + "," + pos + "," + power);
+                }
+            } catch (FileNotFoundException e) {
+                // ignore
+            }
+        }
+
+        List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
+
+        SimpleRegression rampReg = new SimpleRegression(fitStatic);
+        for (int i = 0; i < timeSamples.size(); i++) {
+            double vel = velSamples.get(i);
+            double power = powerSamples.get(i);
+
+            rampReg.addData(vel, power);
+        }
+
+        return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
+                              rampReg.getRSquare());
+    }
+
+    /**
+     * Run regression to compute acceleration feedforward.
+     *
+     * @param timeSamples time samples
+     * @param positionSamples position samples
+     * @param powerSamples power samples
+     * @param rampResult ramp result
+     * @param file log file
+     */
+    public static AccelResult fitAccelData(List<Double> timeSamples, List<Double> positionSamples,
+                                           List<Double> powerSamples, RampResult rampResult,
+                                           @Nullable File file) {
+        if (file != null) {
+            try (PrintWriter pw = new PrintWriter(file)) {
+                pw.println("time,position,power");
+                for (int i = 0; i < timeSamples.size(); i++) {
+                    double time = timeSamples.get(i);
+                    double pos = positionSamples.get(i);
+                    double power = powerSamples.get(i);
+                    pw.println(time + "," + pos + "," + power);
+                }
+            } catch (FileNotFoundException e) {
+                // ignore
+            }
+        }
+
+        List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
+        List<Double> accelSamples = numericalDerivative(timeSamples, velSamples);
+
+        SimpleRegression accelReg = new SimpleRegression(false);
+        for (int i = 0; i < timeSamples.size(); i++) {
+            double vel = velSamples.get(i);
+            double accel = accelSamples.get(i);
+            double power = powerSamples.get(i);
+
+            double powerFromVel = Kinematics.calculateMotorFeedforward(
+                    vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic);
+            double powerFromAccel = power - powerFromVel;
+
+            accelReg.addData(accel, powerFromAccel);
+        }
+
+        return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare());
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java

@@ -0,0 +1,90 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "autoBlue")
+public class AutoBlue extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+    private String randomization;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230));
+    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(220));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
+    //Board Scores
+    final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -26.5, Math.toRadians(180));
+    final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(178));
+    final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(180));
+
+    final static Pose2d BACK_OFF =  new Pose2d(-68, -55,Math.toRadians(180));
+    final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180));
+
+    protected void boardScore() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
+                builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
+                builder.lineToLinearHeading(RIGHT_BOARD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                builder.lineToLinearHeading(CENTER_BOARD);
+                break;
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD);
+                builder.lineToLinearHeading(LEFT_BOARD);
+                break;
+        }
+        builder.addTemporalMarker(.75, robot.getArm()::armScore);
+        builder.addTemporalMarker(2, robot.getSlides()::slideFirstLayer);
+        builder.addTemporalMarker(2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void park() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(BACK_OFF);
+        builder.lineToLinearHeading(PARK);
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getClaw()::close);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+//        this.robot.getCamera().initTargetingCamera();
+        this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+            this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
+            randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
+            this.telemetry.update();
+        }
+        boardScore();
+        sleep(100);
+        this.robot.getClaw().open();
+        sleep(100);
+        park();
+        sleep(300);
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java

@@ -0,0 +1,157 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "autoBlueFar")
+public class AutoBlueFarStem extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+    private String randomization;
+    private String parkLocation;
+    private int delay = 10000;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360));
+    final static Pose2d LEFT_PRELOAD_GETOUT = new Pose2d(43, -42, Math.toRadians(330));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(35, -27, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD = new Pose2d(44, -35, Math.toRadians(270));
+    //Ready Truss
+    final static Pose2d READY_TRUSS = new Pose2d(43, -57, Math.toRadians(180));
+    final static Pose2d READY_TRUSSTEMP = new Pose2d(35, -57, Math.toRadians(180));
+    final static Pose2d TO_BOARD = new Pose2d(-35, -57, Math.toRadians(180));
+    final static Pose2d SCORE_BOARD_LEFT = new Pose2d(-52, -38, Math.toRadians(180));
+    final static Pose2d SCORE_BOARD_MID = new Pose2d(-52, -32, Math.toRadians(180));
+    final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(-52, -27, Math.toRadians(180));
+    final static Pose2d PARK = new Pose2d(-55, -56, Math.toRadians(180));
+    final static Pose2d PARK2 = new Pose2d(-60, -59, Math.toRadians(180));
+    final static Pose2d PARKLEFT = new Pose2d(-45, -10, Math.toRadians(180));
+    final static Pose2d PARKLEFT2 = new Pose2d(-60, -10, Math.toRadians(180));
+
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD_ONE);
+                builder.lineToLinearHeading(LEFT_PRELOAD_TWO);
+                builder.lineToLinearHeading(LEFT_PRELOAD_GETOUT);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD);
+                break;
+        }
+        builder.addTemporalMarker(.3, this.robot.getArm()::armScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void goBackToWhereYouCameFrom() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(READY_TRUSSTEMP);
+//        builder.lineToLinearHeading(TO_BOARD,
+//                MecanumDrive.getVelocityConstraint(70, 70, DriveConstants.TRACK_WIDTH),
+//                MecanumDrive.getAccelerationConstraint(70));
+        builder.addTemporalMarker(.3,robot.getArm()::armRest);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+//    protected void scoreBoard(){
+//        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+//        switch (randomization) {
+//            case "LEFT":
+//                builder.lineToLinearHeading(SCORE_BOARD_LEFT,
+//                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+//                        MecanumDrive.getAccelerationConstraint(20));
+//                break;
+//            case "CENTER":
+//                builder.lineToLinearHeading(SCORE_BOARD_MID,
+//                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+//                        MecanumDrive.getAccelerationConstraint(20));
+//                break;
+//            case "RIGHT":
+//                builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
+//                        MecanumDrive.getVelocityConstraint(30, 30, DriveConstants.TRACK_WIDTH),
+//                        MecanumDrive.getAccelerationConstraint(30));
+//                break;
+//        }
+//        builder.addTemporalMarker(.2, robot.getArm()::armScore);
+//        builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
+//        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+//        this.robot.getDrive().followTrajectorySequence(builder.build());
+//    }
+//
+//    protected void park(){
+//        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+//        switch(parkLocation) {
+//            case "RIGHT":
+//                builder.lineToLinearHeading(PARKLEFT);
+//                builder.lineToLinearHeading(PARKLEFT2);
+//                break;
+//            case "LEFT":
+//                builder.lineToLinearHeading(PARK);
+//                builder.lineToLinearHeading(PARK2);
+//                break;
+//        }
+//        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+//        builder.addTemporalMarker(.1, robot.getClaw()::close);
+//        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+//        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+//        this.robot.getDrive().followTrajectorySequence(builder.build());
+//
+//    }
+
+    protected void delaySet(){
+        if (gamepad2.dpad_up && delay<13000) {
+            delay+=1000;
+            sleep(100);
+        } else if (gamepad2.dpad_down && delay>0) {
+            delay-=1000;
+            sleep(100);
+        }
+    }
+
+        @Override
+    public void runOpMode() throws InterruptedException {
+
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+//        this.robot.getCamera().initTargetingCamera();
+            this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+            this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
+            randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
+            if (gamepad2.dpad_left) {
+                parkLocation="LEFT";
+            } else if (gamepad2.dpad_right) {
+                parkLocation = "RIGHT";
+            }
+            delaySet();
+            this.telemetry.addData("Park Position", parkLocation);
+            this.telemetry.addData("Delay", delay);
+            this.telemetry.update();
+        }
+        sleep(delay);
+        scorePreloadOne();
+        goBackToWhereYouCameFrom();
+//        scoreBoard();
+//        sleep(500);
+//        this.robot.getClaw().open();
+//        sleep(500);
+//        park();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java

@@ -0,0 +1,287 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import static org.firstinspires.ftc.teamcode.hardware.Robot.Slides.SLIDELAYERONE;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "autoBlue2+2")
+public class AutoBlueTwoPlusTwo extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+    private String randomization = "CENTER";
+    private String parkLocation = "LEFT";
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-32, -30, Math.toRadians(180));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -26, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270));
+    //Board Scores
+    final static Pose2d RIGHT_BOARD = new Pose2d(-76.5, -24, Math.toRadians(180));
+    final static Pose2d CENTER_BOARD = new Pose2d(-78, -35, Math.toRadians(184));
+    final static Pose2d LEFT_BOARD = new Pose2d(-78, -41, Math.toRadians(182));
+    //Parka
+    final static Pose2d PARK = new Pose2d(-60, -58, Math.toRadians(180));
+    final static Pose2d PARK2 = new Pose2d(-80, -60, Math.toRadians(180));
+    final static Pose2d PARKLEFT = new Pose2d(-70, -15, Math.toRadians(180));
+    final static Pose2d PARKLEFT2 = new Pose2d(-75, -12, Math.toRadians(180));
+    //Cycles
+    final static Vector2d LEAVE_BOARD = new Vector2d(-50, -10);
+    final static Vector2d TO_STACK = new Vector2d(38, -10);
+    final static Vector2d TO_STACK_SLOW = new Vector2d(47, -9.5);
+    final static Pose2d TO_STACK_SLOW2 = new Pose2d(38.5, -8, Math.toRadians(180));
+    final static Pose2d BACK_THROUGH_GATE = new Pose2d(-50, -10, Math.toRadians(180));
+    final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -31, Math.toRadians(180));
+    final static Vector2d SCORE_STACK = new Vector2d(-72, -30);
+    final static Vector2d SCORE_STACK_SLOW = new Vector2d(-73, -30);
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "RIGHT":
+                builder.setReversed(true);
+                builder.splineToSplineHeading(RIGHT_PRELOAD_TWO, Math.toRadians(360),
+                        MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                builder.setReversed(false);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void boardScore() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_BOARD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                break;
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(30));
+                break;
+        }
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStack() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.setReversed(true);
+        builder.splineToConstantHeading(LEAVE_BOARD, Math.toRadians(360));
+        builder.lineToConstantHeading(TO_STACK,
+                MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(80));
+        builder.setReversed(false);
+        switch (randomization) {
+            case "RIGHT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 2)),
+                        MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(15));
+                break;
+            case "CENTER":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
+                        MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(15));
+                break;
+            case "LEFT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
+                        MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(15));
+                break;
+        }
+        builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
+        builder.addTemporalMarker(.3, robot.getClaw()::close);
+        builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(2, robot.getClaw()::openStack);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStackLower() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.setReversed(true);
+        builder.splineToConstantHeading(LEAVE_BOARD.plus(new Vector2d(0, -1)), Math.toRadians(360));
+        builder.lineToConstantHeading(TO_STACK.plus(new Vector2d(0, -1)),
+                MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(80));
+        builder.setReversed(false);
+        switch (randomization) {
+            case "RIGHT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
+                        MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(15));
+                break;
+            case "CENTER":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
+                        MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(15));
+                break;
+            case "LEFT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
+                        MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(15));
+                break;
+        }
+        builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow);
+        builder.addTemporalMarker(.3, robot.getClaw()::close);
+        builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.7, robot.getClaw()::openStack);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toBoard() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(LEAVE_BOARD);
+        builder.splineToConstantHeading(SCORE_STACK, Math.toRadians(180),
+                MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(40));
+        builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
+        builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack);
+        builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
+        builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStage() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(PARKLEFT2.vec());
+        builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void scoreTest() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(SCORE_STACK_SLOW,
+                MecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(30));
+        builder.addTemporalMarker(.3, this::clawSlowOpen);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void clawSlowOpen() {
+        double currentPos = .61;
+        double targetPos = .58;
+        int slidesPos = SLIDELAYERONE;
+        int slideDelta = 2;
+        double delta = (targetPos - currentPos) / 100;
+        for (int i = 0; i < 100; i++) {
+            this.robot.getClaw().setPos(currentPos + (delta * i));
+            this.robot.getSlides().slideTo(slidesPos + (slideDelta * i), 1);
+            sleep(7);
+        }
+    }
+
+    protected void park() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(PARKLEFT);
+        builder.addTemporalMarker(.01, robot.getArm()::armRest);
+        builder.addTemporalMarker(.01, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.01, robot.getSlides()::slideDown);
+        builder.addTemporalMarker(.01, robot.getClaw()::open);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+//    protected void parkLocation() {
+//        if (gamepad2.dpad_left) {
+//            parkLocation = "LEFT";
+//        } else if (gamepad2.dpad_right) {
+//            parkLocation = "RIGHT";
+//        }
+//    }
+
+    protected void startLocation() {
+        if (gamepad2.x) {
+            randomization = "LEFT";
+        } else if (gamepad2.y) {
+            randomization = "CENTER";
+        } else if (gamepad2.b) {
+            randomization = "RIGHT";
+        }
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+        while(!this.robot.getCamera().isReady()) {
+            sleep(20);
+        }
+//        this.robot.getCamera().setAlliance(Alliance.Blue);
+//        this.robot.getCamera().initTargetingCamera();
+        this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+//            randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
+//            parkLocation();
+            startLocation();
+            this.telemetry.addData("Starting Position", randomization);
+            this.telemetry.addData("Park Position", parkLocation);
+            this.telemetry.update();
+        }
+        robot.update();
+        this.robot.getClaw().close();
+        scorePreloadOne();
+        boardScore();
+//        sleep(250);
+        this.robot.getClaw().open();
+//        sleep(250);
+        toStack();
+        this.robot.getClaw().close();
+        sleep(300);
+        toBoard();
+        clawSlowOpen();
+//            sleep(100);
+        toStackLower();
+        this.robot.getClaw().close();
+        sleep(300);
+        toBoard();
+        clawSlowOpen();
+        park();
+//            park();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java

@@ -0,0 +1,145 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "autoRed")
+public class AutoRed extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+    private String randomization = "CENTER";
+    private String parkLocation = "LEFT";
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(27, -26, Math.toRadians(360));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270));
+    //Board Scores
+    final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360));
+    final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360));
+    final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360));
+    //Parka
+    final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360));
+    final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360));
+    final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360));
+    final static Pose2d PARKLEFT2 = new Pose2d(75, -2, Math.toRadians(360));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.setReversed(true);
+                builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180),
+                        MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                builder.setReversed(false);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void boardScore() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_BOARD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(30));
+                break;
+        }
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void park() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch(parkLocation) {
+            case "LEFT":
+                builder.lineToLinearHeading(PARKLEFT);
+                builder.lineToLinearHeading(PARKLEFT2);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(PARK);
+                builder.lineToLinearHeading(PARK2);
+                break;
+        }
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void parkLocation() {
+        if (gamepad2.dpad_left) {
+            parkLocation = "LEFT";
+        } else if (gamepad2.dpad_right) {
+            parkLocation = "RIGHT";
+        }
+    }
+
+    protected void startLocation() {
+        if (gamepad2.x) {
+            randomization = "LEFT";
+        } else if (gamepad2.y) {
+            randomization = "CENTER";
+        } else if (gamepad2.b) {
+            randomization = "RIGHT";
+        }
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+        this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+            parkLocation();
+            startLocation();
+            this.telemetry.addData("Starting Position", randomization);
+            this.telemetry.addData("Park Position", parkLocation);
+            this.telemetry.update();
+        }
+        robot.update();
+        this.robot.getClaw().close();
+        scorePreloadOne();
+        boardScore();
+        this.robot.getClaw().open();
+        sleep(250);
+        park();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java

@@ -0,0 +1,169 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "AutoRedFar")
+public class AutoRedFar extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+    private String randomization;
+    private String parkLocation;
+    private int delay = 10000;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230));
+    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200));
+    final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(230));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
+    //Ready Truss
+    final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270));
+    final static Pose2d READY_TRUSS = new Pose2d(-43, -57, Math.toRadians(0));
+    final static Pose2d TO_BOARD = new Pose2d(30, -57, Math.toRadians(0));
+    final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0));
+    final static Pose2d SCORE_BOARD_LEFT = new Pose2d(54, -27, Math.toRadians(5));
+    final static Pose2d SCORE_BOARD_MID = new Pose2d(54, -32, Math.toRadians(0));
+    final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(54, -41, Math.toRadians(0));
+    final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0));
+    final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0));
+    final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0));
+    final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
+                builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
+                builder.lineToLinearHeading(RIGHT_PRELOAD_GETOUT);
+                break;
+        }
+        builder.addTemporalMarker(.4,robot.getArm()::armScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+
+
+    protected void goBackToWhereYouCameFrom() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(LEAVE);
+        builder.lineToLinearHeading(READY_TRUSS);
+        builder.lineToLinearHeading(TO_BOARD);
+        builder.addTemporalMarker(.3,robot.getArm()::armRest);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void scoreBoard(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(READY_SCORE);
+
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void score(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(SCORE_BOARD_LEFT,
+                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(20));
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(SCORE_BOARD_MID,
+                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(20));
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
+                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(20));
+                break;
+        }
+        builder.addTemporalMarker(1.8, robot.getClaw()::open);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void park(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch(parkLocation) {
+            case "LEFT":
+                builder.lineToLinearHeading(PARKLEFT);
+                builder.lineToLinearHeading(PARKLEFT2);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(PARK);
+                builder.lineToLinearHeading(PARK2);
+                break;
+        }
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+    }
+
+    protected void parkLocation(){
+    if (gamepad2.dpad_left) {
+        parkLocation="LEFT";
+    } else if (gamepad2.dpad_right) {
+        parkLocation = "RIGHT";
+    }}
+
+    protected void delaySet(){
+        if (gamepad2.dpad_up && delay<13000) {
+            delay+=1000;
+            sleep(100);
+        } else if (gamepad2.dpad_down && delay>0) {
+            delay-=1000;
+            sleep(100);
+        }
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+//        this.robot.getCamera().initTargetingCamera();
+        this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+            parkLocation();
+            delaySet();
+            this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
+            randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
+            this.telemetry.addData("Park Position", parkLocation);
+            this.telemetry.addData("Delay", delay);
+            this.telemetry.update();
+
+        }
+        sleep(delay);
+        scorePreloadOne();
+        goBackToWhereYouCameFrom();
+        scoreBoard();
+        score();
+        sleep(500);
+        park();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFour.java

@@ -0,0 +1,306 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "autoRed2+4")
+public class AutoRedTwoPlusFour extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+    private String randomization = "CENTER";
+    private String parkLocation = "LEFT";
+
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(27, -26, Math.toRadians(360));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270));
+    //Board Scores
+    final static Pose2d LEFT_BOARD = new Pose2d(78, -23.5, Math.toRadians(360));
+    final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360));
+    final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360));
+    //Parka
+    final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360));
+    final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360));
+    final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360));
+    final static Pose2d PARKLEFT2 = new Pose2d(75, -12, Math.toRadians(360));
+    //Cycles
+    final static Vector2d LEAVE_BOARD = new Vector2d(50, -10);
+    final static Vector2d TO_STACK = new Vector2d(-35, -10);
+    final static Vector2d TO_STACK_SLOW = new Vector2d(-47.5, -10);
+    final static Pose2d TO_STACK_SLOW2 = new Pose2d(-38.5, -8, Math.toRadians(360));
+    final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360));
+    final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360));
+    final static Vector2d SCORE_STACK = new Vector2d(72.5, -35);
+    final static Vector2d SCORE_STACK_SLOW = new Vector2d(73, -35);
+    //Park
+    final static Pose2d BACK_OFF = new Pose2d(60, -58, Math.toRadians(360));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.setReversed(true);
+                builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180),
+                        MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                builder.setReversed(false);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void boardScore() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_BOARD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(30));
+                break;
+        }
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStack() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.setReversed(true);
+        builder.splineToConstantHeading(LEAVE_BOARD, Math.toRadians(180));
+        builder.lineToConstantHeading(TO_STACK,
+                MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(80));
+        builder.setReversed(false);
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "CENTER":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 3.2)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "RIGHT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 0.25)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+        }
+        builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
+        builder.addTemporalMarker(.3, robot.getClaw()::close);
+        builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(2, robot.getClaw()::openStack);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStackLower() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.setReversed(true);
+        builder.splineToConstantHeading(LEAVE_BOARD.plus(new Vector2d(0, -1)), Math.toRadians(180));
+        builder.lineToConstantHeading(TO_STACK.plus(new Vector2d(0, -1)),
+                MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(80));
+        builder.setReversed(false);
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 0)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "CENTER":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "RIGHT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -3)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+        }
+        builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow);
+        builder.addTemporalMarker(.3, robot.getClaw()::close);
+        builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.7, robot.getClaw()::openStack);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toBoard() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(LEAVE_BOARD);
+        builder.splineToConstantHeading(SCORE_STACK, Math.toRadians(360),
+                MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(40));
+        builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
+        builder.addTemporalMarker(2.2, robot.getArm()::armScoreStack);
+        builder.addTemporalMarker(2.4, robot.getWrist()::wristScore);
+        builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStage() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(PARKLEFT2.vec());
+        builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void scoreTest() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(SCORE_STACK_SLOW,
+                MecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(30));
+        builder.addTemporalMarker(.3, this::clawSlowOpen);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void clawSlowOpen() {
+        double currentPos = .62;
+        double targetPos = .6;
+        double delta = (targetPos - currentPos) / 100;
+        for (int i = 0; i < 100; i++) {
+            this.robot.getClaw().setPos(currentPos + (delta * i));
+            this.robot.getSlides().slideFirstLayer();
+            sleep(5);
+        }
+    }
+
+    protected void park() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (parkLocation) {
+            case "LEFT":
+                builder.lineToLinearHeading(PARKLEFT);
+                builder.lineToLinearHeading(PARKLEFT2);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(PARK);
+                builder.lineToLinearHeading(PARK2);
+                break;
+        }
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void parkLocation() {
+        if (gamepad2.dpad_left) {
+            parkLocation = "LEFT";
+        } else if (gamepad2.dpad_right) {
+            parkLocation = "RIGHT";
+        }
+    }
+
+    protected void startLocation() {
+        if (gamepad2.x) {
+            randomization = "LEFT";
+        } else if (gamepad2.y) {
+            randomization = "CENTER";
+        } else if (gamepad2.b) {
+            randomization = "RIGHT";
+        }
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+//        this.robot.getCamera().setAlliance(Alliance.Blue);
+//        this.robot.getCamera().initTargetingCamera();
+        this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+//            randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
+            parkLocation();
+            startLocation();
+            this.telemetry.addData("Starting Position", randomization);
+            this.telemetry.addData("Park Position", parkLocation);
+            this.telemetry.update();
+        }
+        robot.update();
+        this.robot.getClaw().close();
+        scorePreloadOne();
+        boardScore();
+//        sleep(250);
+        this.robot.getClaw().open();
+        sleep(250);
+        toStack();
+        this.robot.getClaw().close();
+        sleep(300);
+        toBoard();
+        clawSlowOpen();
+//            sleep(100);
+        toStackLower();
+        this.robot.getClaw().close();
+        sleep(300);
+        switch (randomization) {
+            default:
+                toBoard();
+                clawSlowOpen();
+                break;
+//                case "LEFT":
+//                    toStage();
+//                    break;
+        }
+        while (!isStopRequested()) {
+            this.robot.getArm().armRest(true);
+            this.robot.getWrist().wristPickup();
+            this.robot.getSlides().slideDown();
+            this.robot.getClaw().open();
+            robot.update();
+        }
+//            park();
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFourTest.java

@@ -0,0 +1,315 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "autoRed2+4Test")
+public class AutoRedTwoPlusFourTest extends LinearOpMode {
+    protected Pose2d initialPosition;
+    GamepadEx controller1;
+    GamepadEx controller2;
+    private Robot robot;
+    private String randomization = "CENTER";
+    private Boolean board1 = true;
+    private Boolean board2 = true;
+    private String parkLocation = "LEFT";
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(27, -26, Math.toRadians(360));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270));
+    //Board Scores
+    final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360));
+    final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360));
+    final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360));
+    //Parka
+    final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360));
+    final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360));
+    final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360));
+    final static Pose2d PARKLEFT2 = new Pose2d(75, -10, Math.toRadians(360));
+    //Cycles
+    final static Vector2d LEAVE_BOARD = new Vector2d(50, -10);
+    final static Vector2d TO_STACK = new Vector2d(-35, -10);
+    final static Vector2d TO_STACK_SLOW = new Vector2d(-45, -10);
+    final static Vector2d SCORE_STACK = new Vector2d(72.5, -35);
+
+    //Trajectory Builders
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.setReversed(true);
+                builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180),
+                        MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                builder.setReversed(false);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void boardScore() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_BOARD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(50));
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_BOARD,
+                        MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(30));
+                break;
+        }
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStack() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.setReversed(true);
+        builder.splineToConstantHeading(LEAVE_BOARD, Math.toRadians(180));
+        builder.lineToConstantHeading(TO_STACK,
+                MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(80));
+        builder.setReversed(false);
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "CENTER":
+                builder.lineToConstantHeading(TO_STACK_SLOW,
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "RIGHT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+        }
+        builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
+        builder.addTemporalMarker(.3, robot.getClaw()::close);
+        builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(2, robot.getClaw()::openStack);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStackLower() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.setReversed(true);
+        builder.splineToConstantHeading(LEAVE_BOARD.plus(new Vector2d(0, -1)), Math.toRadians(180));
+        builder.lineToConstantHeading(TO_STACK.plus(new Vector2d(0, -1)),
+                MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(80));
+        builder.setReversed(false);
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 0)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "CENTER":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -.50)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "RIGHT":
+                builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -2)),
+                        MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+        }
+        builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow);
+        builder.addTemporalMarker(.3, robot.getClaw()::close);
+        builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.5, robot.getClaw()::openStack);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toBoard() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(LEAVE_BOARD);
+        builder.splineToConstantHeading(SCORE_STACK, Math.toRadians(360),
+                MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(40));
+        builder.addTemporalMarker(.1, robot.getArm()::armPickdaUpy);
+        builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack);
+        builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
+        builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void toStage() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(PARKLEFT2.vec());
+        builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
+        builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack);
+        builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    protected void clawSlowOpen() {
+        double currentPos = .62;
+        double targetPos = .6;
+        double delta = (targetPos - currentPos) / 100;
+        for (int i = 0; i < 100; i++) {
+            this.robot.getClaw().setPos(currentPos + (delta * i));
+            this.robot.getSlides().slideFirstLayer();
+            sleep(5);
+        }
+    }
+
+    protected void park() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (parkLocation) {
+            case "LEFT":
+                builder.lineToLinearHeading(PARKLEFT);
+                builder.lineToLinearHeading(PARKLEFT2);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(PARK);
+                builder.lineToLinearHeading(PARK2);
+                break;
+        }
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+        while (this.robot.getDrive().isBusy()) {
+            robot.update();
+        }
+    }
+
+    //Init Customization
+    protected void parkLocation() {
+        if (gamepad2.dpad_left) {
+            parkLocation = "LEFT";
+        } else if (gamepad2.dpad_right) {
+            parkLocation = "RIGHT";
+        }
+    }
+
+    protected void startLocation() {
+        if (gamepad2.x) {
+            randomization = "LEFT";
+        } else if (gamepad2.y) {
+            randomization = "CENTER";
+        } else if (gamepad2.b) {
+            randomization = "RIGHT";
+        }
+    }
+
+    protected void cycles() {
+        if (gamepad2.left_bumper) {
+            if (!board1) board1 = true;
+            else board1 = false;
+            sleep(500);
+        } else if (gamepad2.right_bumper) {
+            if (!board2) board2 = true;
+            else board2 = false;
+            sleep(500);
+        }
+    }
+
+    //Super fancy chinese shit
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+//        this.robot.getCamera().setAlliance(Alliance.Blue);
+//        this.robot.getCamera().initTargetingCamera();
+        this.controller2 = new GamepadEx(this.gamepad2);
+        this.controller1 = new GamepadEx(this.gamepad1);
+        this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+//            randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
+            cycles();
+            parkLocation();
+            startLocation();
+            this.telemetry.addData("Randomization", randomization);
+            this.telemetry.addData("Park Position", parkLocation);
+            this.telemetry.addData("Board 1", board1);
+            this.telemetry.addData("Board 2", board2);
+            this.telemetry.update();
+        }
+        robot.update();
+        this.robot.getClaw().close();
+        scorePreloadOne();
+        boardScore();
+        this.robot.getClaw().open();
+        sleep(250);
+        toStack();
+        this.robot.getClaw().close();
+        if (board1) {
+            toBoard();
+            clawSlowOpen();
+        } else {
+            toStage();
+            this.robot.getClaw().openStack();
+        }
+        toStackLower();
+        this.robot.getClaw().close();
+        if (board2) {
+            toBoard();
+            clawSlowOpen();
+        } else {
+            toStage();
+            this.robot.getClaw().openStack();
+        }
+        while (!isStopRequested()) {
+            this.robot.getArm().armRest(true);
+            this.robot.getWrist().wristPickup();
+            this.robot.getSlides().slideDown();
+            this.robot.getClaw().open();
+            robot.update();
+        }
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java

@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+
+@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Main TeleOp", group = "Development")
+public class MainTeleOp extends OpMode {
+    GamepadEx controller1;
+    GamepadEx controller2;
+    private Robot robot;
+    private MecanumDrive drive;
+
+    @Override
+    public void init() {
+        this.robot = new Robot().init(hardwareMap);
+        this.controller2 = new GamepadEx(this.gamepad2);
+        this.controller1 = new GamepadEx(this.gamepad1);
+    }
+
+    public void loop() {
+//GamePad Controls
+        boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP);
+        boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT);
+        boolean slideStop = controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP) || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT);
+        boolean slideSlow = gamepad2.left_trigger > .1;
+        boolean hang = gamepad2.left_bumper;
+        boolean hangRelease = gamepad2.right_bumper;
+        boolean hangPlane = gamepad2.y;
+        boolean plane = gamepad2.dpad_right;
+//Read Controller
+        controller1.readButtons();
+        controller2.readButtons();
+//Drive
+        robot.getDrive().setInput(gamepad1, gamepad2);
+//slides
+        if (slideUp) {
+            this.robot.getSlides().slideUp();
+        } else if (slideDown) {
+            this.robot.getSlides().slideDown();
+        } else if (slideStop) {
+            this.robot.getSlides().slideStop();
+        }
+        //Slide Power
+        if (slideSlow) {
+            Robot.Slides.SLIDE_POWER_UP = .25;
+            Robot.Slides.SLIDE_POWER_DOWN = .2;
+        } else {
+            Robot.Slides.SLIDE_POWER_UP = .7;
+            Robot.Slides.SLIDE_POWER_DOWN = .5;
+        }
+//Macros
+        this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
+        this.robot.armMacro(controller2, getRuntime()); //X
+
+//Arm and Wrist
+//        if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) {
+//            this.robot.getArm().pickup(true);
+//        } else
+//        if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) {
+//            this.robot.getArm().armRest(true);
+//        } else
+        if (controller2.wasJustPressed(GamepadKeys.Button.A)) {
+            this.robot.getArm().armRest();
+            this.robot.getWrist().wristPickup();
+        }
+//Claw
+        if (controller2.wasJustPressed(GamepadKeys.Button.B)) {
+            gamepad1.rumble(300);
+        } else if (controller2.isDown(GamepadKeys.Button.B) || controller2.isDown(GamepadKeys.Button.DPAD_DOWN)) {
+            this.robot.getClaw().open();
+        } else if (controller2.wasJustReleased(GamepadKeys.Button.B)) {
+            this.robot.getClaw().close();
+        }
+//Hang
+        if (hang) {
+            this.robot.getHang().hang();
+        } else if (hangRelease) {
+            this.robot.getHang().hangRelease();
+        } else if (hangPlane) {
+            this.robot.getHang().hangPlane();
+        }
+//Plane
+        if (plane) {
+            this.robot.getPlane().planeLaunch();
+        } else {
+            this.robot.getPlane().planeLock();
+        }
+        this.robot.update();
+//Telemetry
+        int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition();
+        telemetry.addData("positionLeft", (PositionLeft));
+        int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition();
+        telemetry.addData("positionRight", (PositionRight));
+        telemetry.addData("Camera Pos Estimate", (robot.estimatedPose));
+        telemetry.addData("Pos Estimate", (robot.drivePose));
+        telemetry.addData("Difference", (robot.distance));
+        telemetry.update();
+//Update Robot
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedFarTwoPlusTwoTest.java

@@ -0,0 +1,253 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Config
+@Autonomous(name = "TestRedFar2+2")
+public class RedFarTwoPlusTwoTest extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+    private String randomization;
+    private String parkLocation;
+    private int delay = 10000;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230));
+    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200));
+    final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(0));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270));
+    //Ready Truss
+    final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270));
+    final static Pose2d READY_TRUSS = new Pose2d(-43, -56.5, Math.toRadians(0));
+    final static Pose2d TO_BOARD = new Pose2d(26, -56.5, Math.toRadians(0));
+    final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0));
+    final static Pose2d SCORE_BOARD_LEFT = new Pose2d(55, -27, Math.toRadians(5));
+    final static Pose2d SCORE_BOARD_MID = new Pose2d(55.5, -32.2, Math.toRadians(5));
+    final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(55, -39, Math.toRadians(5));
+    final static Pose2d GET_STACK = new Pose2d(-47, -30, Math.toRadians(0));
+    final static Pose2d PICKUP_STACK = new Pose2d(-61.5, -30.5, Math.toRadians(0));
+    final static Pose2d PICKUP_STACK_MID = new Pose2d(-61.5, -32.5, Math.toRadians(0));
+    final static Pose2d READY_SCORESTACK = new Pose2d(50, -41, Math.toRadians(0));
+    final static Pose2d PICKUP_STACK_LEFT = new Pose2d(-60.5, -33, Math.toRadians(0));
+    final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0));
+    final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0));
+    final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0));
+    final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
+                builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void goBackToWhereYouCameFrom() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(LEAVE,
+                MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(90));
+        builder.lineToLinearHeading(READY_TRUSS,
+                MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(90));
+        builder.lineToLinearHeading(TO_BOARD,
+                MecanumDrive.getVelocityConstraint(90, Math.toRadians(90), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(90));
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void scoreBoard(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(READY_SCORE);
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void score(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(SCORE_BOARD_LEFT,
+                        MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(SCORE_BOARD_MID,
+                        MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(20));
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
+                        MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(40));
+                break;
+        }
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void backTruss(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(TO_BOARD);
+        builder.lineToLinearHeading(READY_TRUSS,
+                MecanumDrive.getVelocityConstraint(80, Math.toRadians(80), DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(80));
+        builder.lineToLinearHeading(GET_STACK);
+        switch (randomization){
+            case "MID":
+                builder.lineToLinearHeading(PICKUP_STACK_MID,
+                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(20));
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(PICKUP_STACK,
+                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(20));
+                break;
+            default:
+                builder.lineToLinearHeading(PICKUP_STACK_LEFT,
+                        MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
+                        MecanumDrive.getAccelerationConstraint(20));
+                break;
+        }
+
+        builder.addTemporalMarker(.3, robot.getArm()::armRest);
+        builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
+        builder.addTemporalMarker(1.5, robot.getArm()::armPickupStack);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void goBackstage() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(READY_TRUSS);
+        builder.lineToLinearHeading(TO_BOARD);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void scoreBoardStack(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(READY_SCORESTACK);
+        builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void scoreTest() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(55, -41, Math.toRadians(0)),
+                MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(20));
+        builder.addTemporalMarker(.3,this::clawSlowOpen);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void clawSlowOpen() {
+        double currentPos = .81;
+        double targetPos = .73;
+        double delta = (targetPos - currentPos) / 100;
+        for (int i = 0; i < 100; i++) {
+            this.robot.getClaw().setPos(currentPos + (delta * i));
+            sleep(30);
+        }
+    }
+
+
+    protected void park(){
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch(parkLocation) {
+            case "LEFT":
+                builder.lineToLinearHeading(PARKLEFT);
+                builder.lineToLinearHeading(PARKLEFT2);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(PARK);
+                builder.lineToLinearHeading(PARK2);
+                break;
+        }
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+    }
+
+    protected void parkLocation(){
+    if (gamepad2.dpad_left) {
+        parkLocation="LEFT";
+    } else if (gamepad2.dpad_right) {
+        parkLocation = "RIGHT";
+    }}
+
+    protected void delaySet(){
+        if (gamepad2.dpad_up && delay<13000) {
+            delay+=1000;
+            sleep(100);
+        } else if (gamepad2.dpad_down && delay>0) {
+            delay-=1000;
+            sleep(100);
+        }
+    }
+
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+        this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+            parkLocation = "RIGHT";
+            this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
+            randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
+            this.telemetry.addData("Park Position", parkLocation);
+            this.telemetry.addData("Delay", delay);
+            this.telemetry.update();
+
+        }
+        scorePreloadOne();
+        goBackToWhereYouCameFrom();
+//        scoreBoard();
+        score();
+        this.robot.getClaw().open();
+        backTruss();
+        this.robot.getClaw().close();
+        sleep(200);
+        goBackstage();
+        scoreBoardStack();
+        scoreTest();
+        park();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CenterStageCommon.java

@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode.util;
+
+public class CenterStageCommon {
+    public enum Alliance {Blue, Red}
+
+    public enum PropLocation {Unknown, Left, Center, Right}
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Color.java

@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.util;
+
+public class Color {
+    public double h;
+    public double s;
+    public double v;
+
+    public Color(double h, double s, double v) {
+        this.h = h;
+        this.s = s;
+        this.v = v;
+    }
+
+    public double[] get() {
+        return new double[]{h, s, v};
+    }
+
+    public double getH() {
+        return h;
+    }
+
+    public double getS() {
+        return s;
+    }
+
+    public double getV() {
+        return v;
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java

@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import android.graphics.Color;
+
+import com.tearabite.ielib.vision.ScalarRange;
+
+import org.opencv.core.Scalar;
+
+public class Colors {
+    // CV Color Threshold Constants
+    public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);
+    public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255);
+    public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER);
+    public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2]));
+    public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80);
+    public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255);
+    public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER);
+    public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40);
+    public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255);
+
+    public static OpenCVUtil.LinePaint RED = new OpenCVUtil.LinePaint(Color.RED);
+
+    public static OpenCVUtil.LinePaint BLUE = new OpenCVUtil.LinePaint(Color.BLUE);
+    public static OpenCVUtil.LinePaint BLACK = new OpenCVUtil.LinePaint(Color.BLACK);
+    public static OpenCVUtil.LinePaint WHITE = new OpenCVUtil.LinePaint(Color.WHITE);
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java

@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+@Config
+public class Configurables {
+
+    // CV Color Threshold Constants
+    public static Color FTC_RED_LOWER = new Color(165, 80, 80);
+    public static Color FTC_RED_UPPER = new Color(15, 255, 255);
+    public static Color FTC_BLUE_LOWER = new Color(75, 40, 80);
+    public static Color FTC_BLUE_UPPER = new Color(120, 255, 255);
+    public static Color FTC_WHITE_LOWER = new Color(0, 0, 40);
+    public static Color FTC_WHITE_UPPER = new Color(180, 30, 255);
+
+    //Drive Speed
+    @Config
+    public static class driveSpeed {
+        public static double SPEED = 1;
+        public static double SLOWMODE_SPEED = 0.3;
+        public static double TURN = .75;
+        public static double SLOWMODE_TURN = 0.3;
+    }
+
+    //Camera Stuff
+    @Config
+    public static class camerStuff {
+        public static double CAMERA_OFFSET_X = 0f;
+        public static double DETECTION_AREA_MIN = 0.02f;
+        public static double DETECTION_AREA_MAX = 0.3f;
+        public static double DETECTION_LEFT_X = 0;
+        public static double DETECTION_CENTER_X = .5;
+        public static double DETECTION_RIGHT_X = 1;
+        public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f;
+    }
+
+    //Auto Temp
+    @Config
+    public static class AuToDeV {
+        //Things
+        public static double X1 = 24, Y1 = -28, R1 = 270;
+        public static double X2 = 50, Y2 = -36, R2 = 360;
+        public static double X3 = 60, Y3 = -6, R3 = 360;
+        public static double X4 = 60, Y4 = -6, R4 = 360;
+        public static double X5 = 0, Y5 = 0, R5 = 0;
+
+        //Pose2d
+        public static Pose2d TEMP1 = new Pose2d(X1, Y1, Math.toRadians(R1));
+        public static Pose2d TEMP2 = new Pose2d(X2, Y2, Math.toRadians(R2));
+        public static Pose2d TEMP3 = new Pose2d(X3, Y3, Math.toRadians(R3));
+        public static Pose2d TEMP4 = new Pose2d(X4, Y4, Math.toRadians(R4));
+        public static Pose2d TEMP5 = new Pose2d(X5, Y5, Math.toRadians(R5));
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java

@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import org.firstinspires.ftc.teamcode.vision.Detection;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+public class Constants {
+    // CV Color Constants
+    public static Scalar RED = new Scalar(255, 0, 0);
+    public static Scalar GREEN = new Scalar(0, 255, 0);
+    public static Scalar BLUE = new Scalar(0, 0, 255);
+    public static Scalar WHITE = new Scalar(255, 255, 255);
+    public static Scalar GRAY = new Scalar(80, 80, 80);
+    public static Scalar BLACK = new Scalar(0, 0, 0);
+    public static Scalar ORANGE = new Scalar(255, 165, 0);
+    public static Scalar YELLOW = new Scalar(255, 255, 0);
+    public static Scalar PURPLE = new Scalar(128, 0, 128);
+
+    // CV Structuring Constants
+    public static final Mat STRUCTURING_ELEMENT = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
+    public static final Point ANCHOR = new Point((STRUCTURING_ELEMENT.cols() / 2f), STRUCTURING_ELEMENT.rows() / 2f);
+    public static final int ERODE_DILATE_ITERATIONS = 2;
+    public static final Size BLUR_SIZE = new Size(7, 7);
+
+    // CV Camera Constants
+    public static final int WEBCAM_WIDTH = 320;
+    public static final int WEBCAM_HEIGHT = 240;
+    public static final OpenCvCameraRotation WEBCAM_ROTATION = OpenCvCameraRotation.UPRIGHT;
+
+    // CV Invalid Detection Constants
+    public static final Point INVALID_POINT = new Point(Double.MIN_VALUE, Double.MIN_VALUE);
+    public static final double INVALID_AREA = -1;
+    public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0);
+
+    // Hardware Name Constants
+
+    public static final String CLAW = "claw";
+    public static final String TARGETING_WEBCAM = "Targeting Webcam";
+    public static final String IMU_SENSOR = "imu";
+    public static final String LEFTHANG = "leftHang";
+    public static final String RIGHTHANG = "rightHang";
+    public static final String LEFTARM = "leftArm";
+    public static final String RIGHTARM = "rightArm";
+    public static final String WRIST = "wrist";
+    public static final String PLANE = "plane";
+    public static final String SLIDERIGHT = "slideright";
+    public static final String SLIDELEFT = "slideleft";
+    public static final String HANGRIGHT = "hangright";
+    public static final String HANGLEFT = "hangleft";
+    public static final String LIGHTS = "lights";
+    public static final String TAG_CAMERA = "tagCamera";
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OpenCVUtil.java

@@ -0,0 +1,97 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import android.graphics.Paint;
+
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfInt;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.imgproc.Moments;
+
+import java.util.Collections;
+import java.util.List;
+
+// CV Helper Functions
+public class OpenCVUtil {
+
+    public static String telem = "nothing";
+
+    // Draw a point
+    public static void drawPoint(Mat img, Point point, Scalar color) {
+        Imgproc.circle(img, point, 3, color,  -1);
+    }
+
+    // Get the center of a contour
+    public static Point getCenterOfContour(MatOfPoint contour) {
+        Moments moments = Imgproc.moments(contour);
+        return new Point(moments.m10 / moments.m00, moments.m01/ moments.m00);
+    }
+
+    // Get the bottom left of a contour
+    public static Point getBottomLeftOfContour(MatOfPoint contour) {
+        Rect boundingRect = Imgproc.boundingRect(contour);
+        return new Point(boundingRect.x, boundingRect.y+boundingRect.height);
+    }
+
+    // Get the bottom right of a contour
+    public static Point getBottomRightOfContour(MatOfPoint contour) {
+        Rect boundingRect = Imgproc.boundingRect(contour);
+        return new Point(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height);
+    }
+
+    // Draw a contour
+    public static void drawContour(Mat img, MatOfPoint contour, Scalar color) {
+        Imgproc.drawContours(img, Collections.singletonList(contour), 0, color, 2);
+    }
+
+    // Draw a convex hull around a contour
+    public static void drawConvexHull(Mat img, MatOfPoint contour, Scalar color) {
+        MatOfInt hull =  new MatOfInt();
+        Imgproc.convexHull(contour, hull);
+        Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, 2);
+    }
+
+    // Draw a filled in convex hull around a contour
+    public static void fillConvexHull(Mat img, MatOfPoint contour, Scalar color) {
+        MatOfInt hull =  new MatOfInt();
+        Imgproc.convexHull(contour, hull);
+        Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, -1);
+    }
+
+    // Convert indexes to points that is used in order to draw the contours
+    public static MatOfPoint convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
+        int[] arrIndex = indexes.toArray();
+        Point[] arrContour = contour.toArray();
+        Point[] arrPoints = new Point[arrIndex.length];
+
+        for (int i=0;i<arrIndex.length;i++) {
+            arrPoints[i] = arrContour[arrIndex[i]];
+        }
+
+        MatOfPoint hull = new MatOfPoint();
+        hull.fromArray(arrPoints);
+        return hull;
+    }
+
+    // Get the largest contour out of a list
+    public static MatOfPoint getLargestContour(List<MatOfPoint> contours) {
+        if (contours.size() == 0) {
+            return null;
+        }
+        return getLargestContours(contours, 1).get(0);
+    }
+
+    // Get the top largest contours
+    public static List<MatOfPoint> getLargestContours(List<MatOfPoint> contours, int numContours) {
+        Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a));
+        return contours.subList(0, Math.min(numContours, contours.size()));
+    }
+
+    public static class LinePaint extends Paint {
+        public LinePaint(int red) {
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ScalarRange.java

@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import org.opencv.core.Scalar;
+
+public class ScalarRange {
+    private Scalar upper;
+    private Scalar lower;
+
+    public ScalarRange(Scalar upper, Scalar lower) {
+        this.upper = upper;
+        this.lower = lower;
+    }
+
+    public Scalar getUpper() {
+        return this.upper;
+    }
+
+    public Scalar getLower() {
+        return this.lower;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java

@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
+import static org.firstinspires.ftc.teamcode.util.Constants.TAG_CAMERA;
+import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.tearabite.ielib.common.Alliance;
+import com.tearabite.ielib.localization.AprilTagPoseEstimator;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.opencv.core.Point;
+import org.openftc.easyopencv.OpenCvCamera;
+
+public class Camera {
+    private OpenCvCamera targetingCamera;
+    private PropDetectionPipeline prop;
+    private boolean targetingCameraInitialized;
+    private AprilTagProcessor aprilTag;
+    private VisionPortal aprilTagPortal;
+    private VisionPortal propPortal;
+    private boolean initialized;
+    private static final Pose2d TAG_CAMERA_OFFSETS = new Pose2d(-8.27, -0.95, 0);
+    private AprilTagPoseEstimator aprilTagPoseEstimator;
+    public static float PROP_REJECTION_VERTICAL_UPPER = 40;
+    public static float PROP_REJECTION_VERTICAL_LOWER = 440;
+
+
+    // Constructor
+    public Camera(HardwareMap hardwareMap) {
+        this.init(hardwareMap);
+    }
+
+    private void init(HardwareMap hardwareMap) {
+        this.aprilTagPoseEstimator = AprilTagPoseEstimator.builder()
+                .robotOffset(TAG_CAMERA_OFFSETS)
+                .build();
+        this.aprilTag = new AprilTagProcessor.Builder()
+                .setDrawAxes(true)
+                .setDrawCubeProjection(true)
+                .setDrawTagID(true)
+                .setDrawTagOutline(true)
+                .build();
+        int[] viewportIds = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.HORIZONTAL);
+
+        VisionPortal.Builder aprilTagVisionPortalBuilder = new VisionPortal.Builder();
+        aprilTagVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, TAG_CAMERA));
+        aprilTagVisionPortalBuilder.setLiveViewContainerId(viewportIds[0]);
+        aprilTagVisionPortalBuilder.setAutoStopLiveView(true);
+        aprilTagVisionPortalBuilder.addProcessor(aprilTag);
+        this.aprilTagPortal = aprilTagVisionPortalBuilder.build();
+
+        this.prop = new PropDetectionPipeline();
+        VisionPortal.Builder propVisionPortalBuilder = new VisionPortal.Builder();
+        propVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, TARGETING_WEBCAM));
+        propVisionPortalBuilder.setLiveViewContainerId(viewportIds[1]);
+        propVisionPortalBuilder.setAutoStopLiveView(true);
+        propVisionPortalBuilder.addProcessor(prop);
+        this.propPortal = propVisionPortalBuilder.build();
+
+        this.propPortal.resumeLiveView();
+        this.aprilTagPortal.resumeLiveView();
+        this.initialized = true;
+    }
+
+    public StarterPosition getStartingPosition() {
+        Detection red = this.prop.getRed();
+        Detection blue = this.prop.getBlue();
+
+        double redArea = red.getArea();
+        double blueArea = blue.getArea();
+
+        return getStartingPosition(redArea > blueArea ? red : blue);
+    }
+
+    private static StarterPosition getStartingPosition(Detection detection) {
+        if (detection == null) {
+            return StarterPosition.UNKNOWN;
+        }
+
+        Point center = detection.getCenter();
+        if (center == null) {
+            return StarterPosition.UNKNOWN;
+        }
+
+        double centerX = detection.getCenter().x + 50;
+        if (centerX < 33) {
+            return StarterPosition.LEFT;
+        } else if (centerX < 66) {
+            return StarterPosition.CENTER;
+        } else if (centerX < 100) {
+            return StarterPosition.RIGHT;
+        }
+
+        return StarterPosition.UNKNOWN;
+    }
+
+
+    public enum StarterPosition {
+        UNKNOWN, LEFT, CENTER, RIGHT
+    }
+
+    public AprilTagDetection getBestAprilTagDetection() {
+        return this.aprilTag.getDetections()
+                .stream().max((d1, d2) -> Float.compare(d2.decisionMargin, d1.decisionMargin))
+                .orElse(null);
+    }
+
+    public Pose2d estimatePoseFromAprilTag() {
+        AprilTagDetection detection = this.getBestAprilTagDetection();
+        if (detection == null) {
+            return null;
+        }
+        return this.aprilTagPoseEstimator.estimatePose(detection);
+    }
+
+    public boolean isReady() {
+        return this.propPortal.getCameraState() == VisionPortal.CameraState.CAMERA_DEVICE_READY;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java

@@ -0,0 +1,120 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_LOWER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_UPPER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_LOWER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_UPPER;
+import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR;
+import static org.firstinspires.ftc.teamcode.util.Constants.BLUE;
+import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE;
+import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS;
+import static org.firstinspires.ftc.teamcode.util.Constants.RED;
+import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+import java.util.ArrayList;
+
+public class ColorDetectionPipeline extends OpenCvPipeline {
+    Mat blurred = new Mat();
+    Mat hsv = new Mat();
+    Mat redMask1 = new Mat();
+    Mat redMask2 = new Mat();
+    Mat redMask = new Mat();
+    Mat whiteMask = new Mat();
+    Scalar redGoalLower1;
+    Scalar redGoalUpper1;
+    Scalar redGoalLower2;
+    Scalar redGoalUpper2;
+
+    Mat blueMask1 = new Mat();
+    Mat blueMask2 = new Mat();
+    Mat blueMask = new Mat();
+    Scalar blueGoalLower1;
+    Scalar blueGoalUpper1;
+    Scalar blueGoalLower2;
+    Scalar blueGoalUpper2;
+
+    private Detection red;
+    private Detection blue;
+
+
+    // Init
+    @Override
+    public void init(Mat input) {
+        red = new Detection(input.size(), 0);
+        blue = new Detection(input.size(), 0);
+    }
+
+
+    // Process each frame that is received from the webcam
+    @Override
+    public Mat processFrame(Mat input) {
+        Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
+        Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
+
+        updateRed(input);
+        updateBlue(input);
+
+        return input;
+    }
+
+    // Update the Red Goal Detection
+    private void updateRed(Mat input) {
+        // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
+        redGoalLower1 = new Scalar(FTC_RED_LOWER.getH(), FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
+        redGoalUpper1 = new Scalar(180, FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
+        redGoalLower2 = new Scalar(0, FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
+        redGoalUpper2 = new Scalar(FTC_RED_UPPER.getH(), FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
+        Core.inRange(hsv, redGoalLower1, redGoalUpper1, redMask1);
+        Core.inRange(hsv, redGoalLower2, redGoalUpper2, redMask2);
+        Core.add(redMask1, redMask2, redMask);
+        Imgproc.erode(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+        Imgproc.dilate(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+
+        ArrayList<MatOfPoint> contours = new ArrayList<>();
+        Imgproc.findContours(redMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
+        for (int i = 0; i < contours.size(); i++) {
+            Detection newDetection = new Detection(input.size(), 0, 1f);
+            newDetection.setContour(contours.get(i));
+            newDetection.draw(input, RED);
+        }
+
+        red.setContour(getLargestContour(contours));
+        red.fill(input, RED);
+    }
+
+    private void updateBlue(Mat input) {
+        // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
+        blueGoalLower1 = new Scalar(FTC_BLUE_LOWER.getH(), FTC_BLUE_LOWER.getS(), FTC_BLUE_LOWER.getV());
+        blueGoalUpper1 = new Scalar(FTC_BLUE_UPPER.getH(), FTC_BLUE_UPPER.getS(), FTC_BLUE_UPPER.getV());
+        Core.inRange(hsv, blueGoalLower1, blueGoalUpper1, blueMask);
+        Imgproc.erode(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+        Imgproc.dilate(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+
+        ArrayList<MatOfPoint> contours = new ArrayList<>();
+        Imgproc.findContours(blueMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
+        for (int i = 0; i < contours.size(); i++) {
+            Detection newDetection = new Detection(input.size(), 0, 1f);
+            newDetection.setContour(contours.get(i));
+            newDetection.draw(input, BLUE);
+        }
+
+        blue.setContour(getLargestContour(contours));
+        blue.fill(input, BLUE);
+    }
+
+    public Detection getRed() {
+        return this.red;
+    }
+
+    public Detection getBlue() {
+        return this.blue;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java

@@ -0,0 +1,131 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+import static org.firstinspires.ftc.teamcode.util.Constants.GREEN;
+import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_AREA;
+import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_POINT;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.drawConvexHull;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.drawPoint;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.fillConvexHull;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomLeftOfContour;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomRightOfContour;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getCenterOfContour;
+
+// Class for a Detection
+public class Detection {
+    private double minAreaPx;
+    private double maxAreaPx;
+    private final Size maxSizePx;
+    private double areaPx =  INVALID_AREA;
+    private Point centerPx = INVALID_POINT;
+    private Point bottomLeftPx = INVALID_POINT;
+    private Point bottomRightPx = INVALID_POINT;
+    private MatOfPoint contour;
+
+    // Constructor
+    public Detection(Size frameSize, double minAreaFactor) {
+        this.maxSizePx = frameSize;
+        this.minAreaPx = frameSize.area() * minAreaFactor;
+        this.maxAreaPx = frameSize.area();
+    }
+
+    public Detection(Size frameSize, double minAreaFactor, double maxSizeFactor) {
+        this.maxSizePx = frameSize;
+        this.minAreaPx = frameSize.area() * minAreaFactor;
+        this.maxAreaPx = frameSize.area() * maxSizeFactor;
+    }
+
+    public void setMinArea(double minAreaFactor) {
+        this.minAreaPx = maxSizePx.area() * minAreaFactor;
+    }
+
+    public void setMaxArea(double maxAreaFactor) {
+        this.minAreaPx = maxSizePx.area() * maxAreaFactor;
+    }
+
+    // Draw a convex hull around the current detection on the given image
+    public void draw(Mat img, Scalar color) {
+        if (isValid()) {
+            drawConvexHull(img, contour, color);
+            drawPoint(img, centerPx, GREEN);
+        }
+    }
+
+    // Draw a convex hull around the current detection on the given image
+    public void fill(Mat img, Scalar color) {
+        if (isValid()) {
+            fillConvexHull(img, contour, color);
+            drawPoint(img, centerPx, GREEN);
+        }
+    }
+
+    // Check if the current Detection is valid
+    public boolean isValid() {
+        return (this.contour != null) && (this.centerPx != INVALID_POINT) && (this.areaPx != INVALID_AREA);
+    }
+
+    // Get the current contour
+    public MatOfPoint getContour() {
+        return contour;
+    }
+
+    // Set the values of the current contour
+    public void setContour(MatOfPoint contour) {
+        this.contour = contour;
+
+        double area;
+        if (contour != null && (area = Imgproc.contourArea(contour)) > minAreaPx && area < maxAreaPx) {
+            this.areaPx = area;
+            this.centerPx = getCenterOfContour(contour);
+            this.bottomLeftPx = getBottomLeftOfContour(contour);
+            this.bottomRightPx = getBottomRightOfContour(contour);
+        } else {
+            this.areaPx = INVALID_AREA;
+            this.centerPx = INVALID_POINT;
+            this.bottomLeftPx = INVALID_POINT;
+            this.bottomRightPx = INVALID_POINT;
+        }
+    }
+
+    // Returns the center of the Detection, normalized so that the width and height of the frame is from [-50,50]
+    public Point getCenter() {
+        if (!isValid()) {
+            return INVALID_POINT;
+        }
+
+        double normalizedX = ((centerPx.x / maxSizePx.width) * 100) - 50;
+        double normalizedY = ((centerPx.y / maxSizePx.height) * -100) + 50;
+
+        return new Point(normalizedX, normalizedY);
+    }
+
+    // Get the center point in pixels
+    public Point getCenterPx() {
+        return centerPx;
+    }
+
+    // Get the area of the Detection, normalized so that the area of the frame is 100
+    public double getArea() {
+        if (!isValid()) {
+            return INVALID_AREA;
+        }
+
+        return (areaPx / (maxSizePx.width * maxSizePx.height)) * 100;
+    }
+
+    // Get the leftmost bottom corner of the detection
+    public Point getBottomLeftCornerPx() {
+        return bottomLeftPx;
+    }
+
+    // Get the rightmost bottom corner of the detection
+    public Point getBottomRightCornerPx() {
+        return bottomRightPx;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java

@@ -0,0 +1,137 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import static org.firstinspires.ftc.teamcode.util.Colors.FTC_BLUE_RANGE;
+import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_1;
+import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_2;
+import static org.firstinspires.ftc.teamcode.util.Colors.RED;
+import static org.firstinspires.ftc.teamcode.util.Colors.WHITE;
+import static org.firstinspires.ftc.teamcode.util.Configurables.camerStuff.DETECTION_AREA_MAX;
+import static org.firstinspires.ftc.teamcode.util.Configurables.camerStuff.DETECTION_AREA_MIN;
+import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR;
+import static org.firstinspires.ftc.teamcode.util.Constants.BLACK;
+import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE;
+import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS;
+import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT;
+import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour;
+import static org.firstinspires.ftc.teamcode.vision.Camera.PROP_REJECTION_VERTICAL_LOWER;
+import static org.firstinspires.ftc.teamcode.vision.Camera.PROP_REJECTION_VERTICAL_UPPER;
+
+import android.graphics.Canvas;
+
+import com.tearabite.ielib.common.Alliance;
+import com.tearabite.ielib.vision.ScalarRange;
+
+import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
+import org.firstinspires.ftc.vision.VisionProcessor;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Point;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+import java.util.ArrayList;
+
+public class PropDetectionPipeline implements VisionProcessor {
+
+    Alliance alliance;
+    private Mat blurred = new Mat();
+    private Mat hsv = new Mat();
+    private Mat mask = new Mat();
+    private Mat tmpMask = new Mat();
+    private Detection red;
+    private Detection blue;
+
+    // Init
+    @Override
+    public void init(int width, int height, CameraCalibration calibration) {
+        this.red = new Detection(new Size(width, height), DETECTION_AREA_MIN, DETECTION_AREA_MAX);
+        this.blue = new Detection(new Size(width, height), DETECTION_AREA_MIN, DETECTION_AREA_MAX);
+    }
+
+    // Process each frame that is received from the webcam
+    @Override
+    public Object processFrame(Mat input, long captureTimeNanos) {
+        Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
+        Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
+
+        red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2));
+        blue.setContour(detect(FTC_BLUE_RANGE));
+
+        return input;
+    }
+
+    private Mat zeros;
+
+    private Mat zeros(Size size, int type) {
+        if (this.zeros == null) {
+            this.zeros = Mat.zeros(size, type);
+        }
+
+        return this.zeros;
+    }
+
+    private MatOfPoint detect(ScalarRange... colorRanges) {
+        mask.release();
+        for (ScalarRange colorRange : colorRanges) {
+            Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask);
+            if (mask.empty() || mask.rows() <= 0) {
+                Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask);
+            }
+            Core.add(mask, tmpMask, mask);
+        }
+
+        Imgproc.rectangle(mask, new Point(0, 0), new Point(mask.cols() - 1, (int) PROP_REJECTION_VERTICAL_UPPER), BLACK, -1);
+        Imgproc.rectangle(mask, new Point(0, (int) PROP_REJECTION_VERTICAL_LOWER), new Point(mask.cols() - 1, mask.rows() - 1), BLACK, -1);
+
+        Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+        Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+
+        ArrayList<MatOfPoint> contours = new ArrayList<>();
+        Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
+
+        return getLargestContour(contours);
+    }
+
+    @Override
+    public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
+        canvas.drawLine(0, PROP_REJECTION_VERTICAL_LOWER, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER, WHITE);
+        canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER, WHITE);
+
+        if (red != null && red.isValid()) {
+            Point center = red.getCenterPx();
+            if (center.y < PROP_REJECTION_VERTICAL_LOWER
+                    && center.y > PROP_REJECTION_VERTICAL_UPPER) {
+                canvas.drawCircle((float) red.getCenterPx().x, (float) red.getCenterPx().y, 10, WHITE);
+            } else {
+                canvas.drawCircle((float) red.getCenterPx().x, (float) red.getCenterPx().y, 10, RED);
+            }
+        }
+
+        if (blue != null && blue.isValid()) {
+            Point center = blue.getCenterPx();
+            if (center.y < PROP_REJECTION_VERTICAL_LOWER
+                    && center.y > PROP_REJECTION_VERTICAL_UPPER) {
+                canvas.drawCircle((float) blue.getCenterPx().x, (float) blue.getCenterPx().y, 10, WHITE);
+            } else {
+                canvas.drawCircle((float) blue.getCenterPx().x, (float) blue.getCenterPx().y, 10, RED);
+            }
+        }
+    }
+
+    public Detection getRed() {
+        return this.red;
+    }
+
+    public Detection getBlue() {
+        return this.blue;
+    }
+
+    public void setAlliance(Alliance alliance) {
+        this.alliance = alliance;
+    }
+
+    public Alliance getAlliance() {
+        return this.alliance;
+    }
+}

build.common.gradle

@@ -57,6 +57,8 @@ android {
         minSdkVersion 24
         //noinspection ExpiredTargetSdkVersion
         targetSdkVersion 28
+        multiDexEnabled true
+
 
         /**
          * We keep the versionCode and versionName of robot controller applications in sync with

build.dependencies.gradle

@@ -1,21 +1,30 @@
 repositories {
     mavenCentral()
     google() // Needed for androidx
+    maven {
+        url = 'https://maven.brott.dev/'
+    }
 }
 
 dependencies {
-    implementation 'org.firstinspires.ftc:Inspection:9.1.0'
-    implementation 'org.firstinspires.ftc:Blocks:9.1.0'
-    implementation 'org.firstinspires.ftc:Tfod:9.1.0'
-    implementation 'org.firstinspires.ftc:RobotCore:9.1.0'
-    implementation 'org.firstinspires.ftc:RobotServer:9.1.0'
-    implementation 'org.firstinspires.ftc:OnBotJava:9.1.0'
-    implementation 'org.firstinspires.ftc:Hardware:9.1.0'
-    implementation 'org.firstinspires.ftc:FtcCommon:9.1.0'
-    implementation 'org.firstinspires.ftc:Vision:9.1.0'
+    implementation 'org.firstinspires.ftc:Inspection:9.0.0'
+    implementation 'org.firstinspires.ftc:Blocks:9.0.0'
+    implementation 'org.firstinspires.ftc:Tfod:9.0.0'
+    implementation 'org.firstinspires.ftc:RobotCore:9.0.0'
+    implementation 'org.firstinspires.ftc:RobotServer:9.0.0'
+    implementation 'org.firstinspires.ftc:OnBotJava:9.0.0'
+    implementation 'org.firstinspires.ftc:Hardware:9.0.0'
+    implementation 'org.firstinspires.ftc:FtcCommon:9.0.0'
+    implementation 'org.firstinspires.ftc:Vision:9.0.0'
     implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0'
     implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3'
     runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0'
     implementation 'androidx.appcompat:appcompat:1.2.0'
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.12'
+    implementation project(':ielib-core')
+    implementation project(':ielib')
+
+    compileOnly 'org.projectlombok:lombok:1.18.30'
+    annotationProcessor 'org.projectlombok:lombok:1.18.30'
 }
 

build.gradle

@@ -11,7 +11,7 @@ buildscript {
     }
     dependencies {
         // Note for FTC Teams: Do not modify this yourself.
-        classpath 'com.android.tools.build:gradle:7.2.0'
+        classpath 'com.android.tools.build:gradle:7.3.1'
     }
 }
 

settings.gradle

@@ -1,2 +1,6 @@
 include ':FtcRobotController'
 include ':TeamCode'
+include ':ielib-core'
+project(':ielib-core').projectDir = new File('../ielib-core')
+include ':ielib'
+project(':ielib').projectDir = new File('../ielib')