Merge remote-tracking branch 'origin/optimus_dev' into optimus_dev

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java

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

@@ -1,189 +1,189 @@
-/* Copyright (c) 2023 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 org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
-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 java.util.List;
-
-/*
- * This OpMode illustrates the basics of AprilTag recognition and pose estimation,
- * including Java Builder structures for specifying Vision parameters.
- *
- * 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 = "Concept: AprilTag", group = "Concept")
-@Disabled
-public class ConceptAprilTag extends LinearOpMode {
-
-    private static final boolean USE_WEBCAM = true;  // true for webcam, false for phone camera
-
-    /**
-     * The variable to store our instance of the AprilTag processor.
-     */
-    private AprilTagProcessor aprilTag;
-
-    /**
-     * The variable to store our instance of the vision portal.
-     */
-    private VisionPortal visionPortal;
-
-    @Override
-    public void runOpMode() {
-
-        initAprilTag();
-
-        // Wait for the DS start button to be touched.
-        telemetry.addData("DS preview on/off", "3 dots, Camera Stream");
-        telemetry.addData(">", "Touch Play to start OpMode");
-        telemetry.update();
-        waitForStart();
-
-        if (opModeIsActive()) {
-            while (opModeIsActive()) {
-
-                telemetryAprilTag();
-
-                // Push telemetry to the Driver Station.
-                telemetry.update();
-
-                // Save CPU resources; can resume streaming when needed.
-                if (gamepad1.dpad_down) {
-                    visionPortal.stopStreaming();
-                } else if (gamepad1.dpad_up) {
-                    visionPortal.resumeStreaming();
-                }
-
-                // Share the CPU.
-                sleep(20);
-            }
-        }
-
-        // Save more CPU resources when camera is no longer needed.
-        visionPortal.close();
-
-    }   // end method runOpMode()
-
-    /**
-     * Initialize the AprilTag processor.
-     */
-    private void initAprilTag() {
-
-        // Create the AprilTag processor.
-        aprilTag = new AprilTagProcessor.Builder()
-            //.setDrawAxes(false)
-            //.setDrawCubeProjection(false)
-            //.setDrawTagOutline(true)
-            //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
-            //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary())
-            //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
-
-            // == CAMERA CALIBRATION ==
-            // 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();
-
-        // Create the vision portal by using a builder.
-        VisionPortal.Builder builder = new VisionPortal.Builder();
-
-        // Set the camera (webcam vs. built-in RC phone camera).
-        if (USE_WEBCAM) {
-            builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"));
-        } else {
-            builder.setCamera(BuiltinCameraDirection.BACK);
-        }
-
-        // Choose a camera resolution. Not all cameras support all resolutions.
-        //builder.setCameraResolution(new Size(640, 480));
-
-        // Enable the RC preview (LiveView).  Set "false" to omit camera monitoring.
-        //builder.enableCameraMonitoring(true);
-
-        // Set the stream format; MJPEG uses less bandwidth than default YUY2.
-        //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2);
-
-        // Choose whether or not LiveView stops if no processors are enabled.
-        // If set "true", monitor shows solid orange screen if no processors enabled.
-        // If set "false", monitor shows camera view without annotations.
-        //builder.setAutoStopLiveView(false);
-
-        // Set and enable the processor.
-        builder.addProcessor(aprilTag);
-
-        // Build the Vision Portal, using the above settings.
-        visionPortal = builder.build();
-
-        // Disable or re-enable the aprilTag processor at any time.
-        //visionPortal.setProcessorEnabled(aprilTag, true);
-
-    }   // end method initAprilTag()
-
-
-    /**
-     * Add telemetry about AprilTag detections.
-     */
-    private void telemetryAprilTag() {
-
-        List<AprilTagDetection> currentDetections = aprilTag.getDetections();
-        telemetry.addData("# AprilTags Detected", currentDetections.size());
-
-        // Step through the list of detections and display info for each one.
-        for (AprilTagDetection detection : currentDetections) {
-            if (detection.metadata != null) {
-                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
-                telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
-                telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
-                telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
-            } else {
-                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
-                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
-            }
-        }   // end for() loop
-
-        // Add "key" information to telemetry
-        telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
-        telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
-        telemetry.addLine("RBE = Range, Bearing & Elevation");
-
-    }   // end method telemetryAprilTag()
-
-}   // end class
+///* Copyright (c) 2023 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 org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+//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 java.util.List;
+//
+///*
+// * This OpMode illustrates the basics of AprilTag recognition and pose estimation,
+// * including Java Builder structures for specifying Vision parameters.
+// *
+// * 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 = "Concept: AprilTag", group = "Concept")
+//@Disabled
+//public class ConceptAprilTag extends LinearOpMode {
+//
+//    private static final boolean USE_WEBCAM = true;  // true for webcam, false for phone camera
+//
+//    /**
+//     * The variable to store our instance of the AprilTag processor.
+//     */
+//    private AprilTagProcessor aprilTag;
+//
+//    /**
+//     * The variable to store our instance of the vision portal.
+//     */
+//    private VisionPortal visionPortal;
+//
+//    @Override
+//    public void runOpMode() {
+//
+//        initAprilTag();
+//
+//        // Wait for the DS start button to be touched.
+//        telemetry.addData("DS preview on/off", "3 dots, Camera Stream");
+//        telemetry.addData(">", "Touch Play to start OpMode");
+//        telemetry.update();
+//        waitForStart();
+//
+//        if (opModeIsActive()) {
+//            while (opModeIsActive()) {
+//
+//                telemetryAprilTag();
+//
+//                // Push telemetry to the Driver Station.
+//                telemetry.update();
+//
+//                // Save CPU resources; can resume streaming when needed.
+//                if (gamepad1.dpad_down) {
+//                    visionPortal.stopStreaming();
+//                } else if (gamepad1.dpad_up) {
+//                    visionPortal.resumeStreaming();
+//                }
+//
+//                // Share the CPU.
+//                sleep(20);
+//            }
+//        }
+//
+//        // Save more CPU resources when camera is no longer needed.
+//        visionPortal.close();
+//
+//    }   // end method runOpMode()
+//
+//    /**
+//     * Initialize the AprilTag processor.
+//     */
+//    private void initAprilTag() {
+//
+//        // Create the AprilTag processor.
+//        aprilTag = new AprilTagProcessor.Builder()
+//            //.setDrawAxes(false)
+//            //.setDrawCubeProjection(false)
+//            //.setDrawTagOutline(true)
+//            //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
+//            //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary())
+//            //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
+//
+//            // == CAMERA CALIBRATION ==
+//            // 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();
+//
+//        // Create the vision portal by using a builder.
+//        VisionPortal.Builder builder = new VisionPortal.Builder();
+//
+//        // Set the camera (webcam vs. built-in RC phone camera).
+//        if (USE_WEBCAM) {
+//            builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"));
+//        } else {
+//            builder.setCamera(BuiltinCameraDirection.BACK);
+//        }
+//
+//        // Choose a camera resolution. Not all cameras support all resolutions.
+//        //builder.setCameraResolution(new Size(640, 480));
+//
+//        // Enable the RC preview (LiveView).  Set "false" to omit camera monitoring.
+//        //builder.enableCameraMonitoring(true);
+//
+//        // Set the stream format; MJPEG uses less bandwidth than default YUY2.
+//        //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2);
+//
+//        // Choose whether or not LiveView stops if no processors are enabled.
+//        // If set "true", monitor shows solid orange screen if no processors enabled.
+//        // If set "false", monitor shows camera view without annotations.
+//        //builder.setAutoStopLiveView(false);
+//
+//        // Set and enable the processor.
+//        builder.addProcessor(aprilTag);
+//
+//        // Build the Vision Portal, using the above settings.
+//        visionPortal = builder.build();
+//
+//        // Disable or re-enable the aprilTag processor at any time.
+//        //visionPortal.setProcessorEnabled(aprilTag, true);
+//
+//    }   // end method initAprilTag()
+//
+//
+//    /**
+//     * Add telemetry about AprilTag detections.
+//     */
+//    private void telemetryAprilTag() {
+//
+//        List<AprilTagDetection> currentDetections = aprilTag.getDetections();
+//        telemetry.addData("# AprilTags Detected", currentDetections.size());
+//
+//        // Step through the list of detections and display info for each one.
+//        for (AprilTagDetection detection : currentDetections) {
+//            if (detection.metadata != null) {
+//                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
+//                telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
+//                telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
+//                telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
+//            } else {
+//                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
+//                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
+//            }
+//        }   // end for() loop
+//
+//        // Add "key" information to telemetry
+//        telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
+//        telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
+//        telemetry.addLine("RBE = Range, Bearing & Elevation");
+//
+//    }   // end method telemetryAprilTag()
+//
+//}   // end class

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

@@ -1,149 +1,149 @@
-/* Copyright (c) 2023 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 org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
-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 java.util.List;
-
-/*
- * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using
- * the easy way.
- *
- * 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 = "Concept: AprilTag Easy", group = "Concept")
-@Disabled
-public class ConceptAprilTagEasy extends LinearOpMode {
-
-    private static final boolean USE_WEBCAM = true;  // true for webcam, false for phone camera
-
-    /**
-     * The variable to store our instance of the AprilTag processor.
-     */
-    private AprilTagProcessor aprilTag;
-
-    /**
-     * The variable to store our instance of the vision portal.
-     */
-    private VisionPortal visionPortal;
-
-    @Override
-    public void runOpMode() {
-
-        initAprilTag();
-
-        // Wait for the DS start button to be touched.
-        telemetry.addData("DS preview on/off", "3 dots, Camera Stream");
-        telemetry.addData(">", "Touch Play to start OpMode");
-        telemetry.update();
-        waitForStart();
-
-        if (opModeIsActive()) {
-            while (opModeIsActive()) {
-
-                telemetryAprilTag();
-
-                // Push telemetry to the Driver Station.
-                telemetry.update();
-
-                // Save CPU resources; can resume streaming when needed.
-                if (gamepad1.dpad_down) {
-                    visionPortal.stopStreaming();
-                } else if (gamepad1.dpad_up) {
-                    visionPortal.resumeStreaming();
-                }
-
-                // Share the CPU.
-                sleep(20);
-            }
-        }
-
-        // Save more CPU resources when camera is no longer needed.
-        visionPortal.close();
-
-    }   // end method runOpMode()
-
-    /**
-     * Initialize the AprilTag processor.
-     */
-    private void initAprilTag() {
-
-        // Create the AprilTag processor the easy way.
-        aprilTag = AprilTagProcessor.easyCreateWithDefaults();
-
-        // Create the vision portal the easy way.
-        if (USE_WEBCAM) {
-            visionPortal = VisionPortal.easyCreateWithDefaults(
-                hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag);
-        } else {
-            visionPortal = VisionPortal.easyCreateWithDefaults(
-                BuiltinCameraDirection.BACK, aprilTag);
-        }
-
-    }   // end method initAprilTag()
-
-    /**
-     * Add telemetry about AprilTag detections.
-     */
-    private void telemetryAprilTag() {
-
-        List<AprilTagDetection> currentDetections = aprilTag.getDetections();
-        telemetry.addData("# AprilTags Detected", currentDetections.size());
-
-        // Step through the list of detections and display info for each one.
-        for (AprilTagDetection detection : currentDetections) {
-            if (detection.metadata != null) {
-                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
-                telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
-                telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
-                telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
-            } else {
-                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
-                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
-            }
-        }   // end for() loop
-
-        // Add "key" information to telemetry
-        telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
-        telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
-        telemetry.addLine("RBE = Range, Bearing & Elevation");
-
-    }   // end method telemetryAprilTag()
-
-}   // end class
+///* Copyright (c) 2023 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 org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+//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 java.util.List;
+//
+///*
+// * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using
+// * the easy way.
+// *
+// * 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 = "Concept: AprilTag Easy", group = "Concept")
+//@Disabled
+//public class ConceptAprilTagEasy extends LinearOpMode {
+//
+//    private static final boolean USE_WEBCAM = true;  // true for webcam, false for phone camera
+//
+//    /**
+//     * The variable to store our instance of the AprilTag processor.
+//     */
+//    private AprilTagProcessor aprilTag;
+//
+//    /**
+//     * The variable to store our instance of the vision portal.
+//     */
+//    private VisionPortal visionPortal;
+//
+//    @Override
+//    public void runOpMode() {
+//
+//        initAprilTag();
+//
+//        // Wait for the DS start button to be touched.
+//        telemetry.addData("DS preview on/off", "3 dots, Camera Stream");
+//        telemetry.addData(">", "Touch Play to start OpMode");
+//        telemetry.update();
+//        waitForStart();
+//
+//        if (opModeIsActive()) {
+//            while (opModeIsActive()) {
+//
+//                telemetryAprilTag();
+//
+//                // Push telemetry to the Driver Station.
+//                telemetry.update();
+//
+//                // Save CPU resources; can resume streaming when needed.
+//                if (gamepad1.dpad_down) {
+//                    visionPortal.stopStreaming();
+//                } else if (gamepad1.dpad_up) {
+//                    visionPortal.resumeStreaming();
+//                }
+//
+//                // Share the CPU.
+//                sleep(20);
+//            }
+//        }
+//
+//        // Save more CPU resources when camera is no longer needed.
+//        visionPortal.close();
+//
+//    }   // end method runOpMode()
+//
+//    /**
+//     * Initialize the AprilTag processor.
+//     */
+//    private void initAprilTag() {
+//
+//        // Create the AprilTag processor the easy way.
+//        aprilTag = AprilTagProcessor.easyCreateWithDefaults();
+//
+//        // Create the vision portal the easy way.
+//        if (USE_WEBCAM) {
+//            visionPortal = VisionPortal.easyCreateWithDefaults(
+//                hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag);
+//        } else {
+//            visionPortal = VisionPortal.easyCreateWithDefaults(
+//                BuiltinCameraDirection.BACK, aprilTag);
+//        }
+//
+//    }   // end method initAprilTag()
+//
+//    /**
+//     * Add telemetry about AprilTag detections.
+//     */
+//    private void telemetryAprilTag() {
+//
+//        List<AprilTagDetection> currentDetections = aprilTag.getDetections();
+//        telemetry.addData("# AprilTags Detected", currentDetections.size());
+//
+//        // Step through the list of detections and display info for each one.
+//        for (AprilTagDetection detection : currentDetections) {
+//            if (detection.metadata != null) {
+//                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
+//                telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
+//                telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
+//                telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
+//            } else {
+//                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
+//                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
+//            }
+//        }   // end for() loop
+//
+//        // Add "key" information to telemetry
+//        telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
+//        telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
+//        telemetry.addLine("RBE = Range, Bearing & Elevation");
+//
+//    }   // end method telemetryAprilTag()
+//
+//}   // end class

MeepMeepTesting/.gitignore

@@ -0,0 +1 @@
+/build

MeepMeepTesting/build.gradle

@@ -0,0 +1,17 @@
+plugins {
+    id 'java-library'
+}
+
+java {
+    sourceCompatibility = JavaVersion.VERSION_1_8
+    targetCompatibility = JavaVersion.VERSION_1_8
+}
+
+repositories {
+    maven { url = 'https://jitpack.io' }
+    maven { url = 'https://maven.brott.dev/' }
+}
+
+dependencies {
+    implementation 'com.github.NoahBres:MeepMeep:2.0.3'
+}

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java

@@ -0,0 +1,131 @@
+package com.example.meepmeeptesting;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.noahbres.meepmeep.MeepMeep;
+import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder;
+import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity;
+
+public class MeepMeepTesting {
+    public static void main(String[] args) {
+        MeepMeep meepMeep = new MeepMeep(800);
+
+        RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
+                // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
+                .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15)
+                .followTrajectorySequence(drive ->
+                        drive.trajectorySequenceBuilder(new Pose2d(-51.5, 33.6,Math.toRadians(180)))
+//                                .lineToConstantHeading(new Vector2d(-45,59.5))
+                                .setTangent(Math.toRadians(90))
+                                .splineToConstantHeading(new Pose2d(-48,59.5).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33,59.5).vec())
+                                .splineToConstantHeading(new Pose2d(52.5, 36).vec(),Math.toRadians(0))
+                                .build()
+                );
+
+        RoadRunnerBotEntity BlueFrontStage = new DefaultBotBuilder(meepMeep)
+                .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
+                        .followTrajectorySequence(drive ->
+                                drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
+                                //Score then pick up 1 white
+                                .lineToLinearHeading(new Pose2d(-39.7, 33.5, Math.toRadians(-90)))
+                                .lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
+
+                                //Spline to board
+                                .setTangent(Math.toRadians(90))
+                                .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0))
+
+                                //Go back to white stack
+                                .splineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
+                                .lineToConstantHeading(new Pose2d(-45, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
+                                .splineToConstantHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
+
+                                //Go back to board
+                                .setTangent(Math.toRadians(90))
+                                .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0))
+                                .build()
+                );
+
+        RoadRunnerBotEntity BlueFrontStage3 = new DefaultBotBuilder(meepMeep)
+                .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
+                .followTrajectorySequence(drive ->
+                        drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
+                                //Score then pick up 1 white
+                                .lineToLinearHeading(new Pose2d(-46.7, 39.5, Math.toRadians(-90)))
+                                .lineToLinearHeading(new Pose2d(-46.7,50.5,Math.toRadians(-90)))
+                                .lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
+
+                                //Spline to board
+                                .setTangent(Math.toRadians(90))
+                                .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0))
+
+                                //Park
+                                .lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
+                                .build()
+                );
+
+        RoadRunnerBotEntity BlueFrontStage1 = new DefaultBotBuilder(meepMeep)
+                .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
+                .followTrajectorySequence(drive ->
+                        drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
+                                //Score then pick up 1 white
+                                .lineToLinearHeading(new Pose2d(-36, 45.5, Math.toRadians(-90)))
+                                .lineToLinearHeading(new Pose2d(-36,33.5,Math.toRadians(0)))
+                                .lineToLinearHeading(new Pose2d(-32,33.5,Math.toRadians(0)))
+                                .lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
+
+                                //Spline to board
+                                .setTangent(Math.toRadians(90))
+                                .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0))
+
+                                //Park
+                                .lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
+                                .build()
+                );
+
+        RoadRunnerBotEntity RedFrontStage = new DefaultBotBuilder(meepMeep)
+                .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
+                .followTrajectorySequence(drive ->
+                        drive.trajectorySequenceBuilder(new Pose2d(-36, -61.5, Math.toRadians(90)))
+                                //Score then pick up 1 white
+                                .lineToLinearHeading(new Pose2d(-39.7, -33.5, Math.toRadians(90)))
+                                .lineToLinearHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,1.5))).waitSeconds(.01)
+
+                                //Spline to board
+                                .setTangent(Math.toRadians(-90))
+                                .splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
+
+                                //Go back to white stack
+                                .splineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
+                                .lineToConstantHeading(new Pose2d(-45, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
+                                .splineToConstantHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
+
+                                //Go back to board
+                                .setTangent(Math.toRadians(-90))
+                                .splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
+                                .build()
+                );
+
+        meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK)
+                .setDarkMode(true)
+                .setBackgroundAlpha(0.95f)
+                //.addEntity(myBot)
+                //.addEntity(BlueFrontStage)
+                //.addEntity(BlueFrontStage3)
+                //.addEntity(BlueFrontStage1)
+                .addEntity(RedFrontStage)
+                .start();
+    }
+}

TeamCode/build.gradle

@@ -10,8 +10,6 @@
 
 
 // Custom definitions may go here
-
-// Include common definitions from above.
 apply from: '../build.common.gradle'
 apply from: '../build.dependencies.gradle'
 
@@ -26,4 +24,10 @@ android {
 dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
+    implementation 'org.openftc:easyopencv:1.7.0'
+    implementation 'org.ftclib.ftclib:vision:2.0.1'
+    implementation 'org.ftclib.ftclib:core:2.0.1'
+    implementation 'org.apache.commons:commons-math3:3.6.1'
+    implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
+    implementation 'com.acmerobotics.roadrunner:core:0.5.6'
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Button.java

@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.controller;
+
+public class Button {
+    boolean currentState = false;
+    boolean lastState = false;
+    boolean justPressed = false;
+    boolean justReleased = false;
+
+    public Button() {}
+
+    public void update(boolean pressed) {
+        lastState = currentState;
+        currentState = pressed;
+
+        justPressed = currentState && !lastState;
+        justReleased = !currentState && lastState;
+    }
+
+    public boolean isPressed() {
+        return currentState;
+    }
+
+    public boolean isJustPressed() {
+        return justPressed;
+    }
+
+    public boolean isJustReleased() {
+        return justReleased;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Controller.java

@@ -0,0 +1,197 @@
+package org.firstinspires.ftc.teamcode.controller;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+public class Controller {
+    private final Gamepad gamepad;
+
+    private final Joystick leftStick;
+    private final Joystick rightStick;
+
+    private final Button leftStickButton;
+    private final Button rightStickButton;
+
+    private final Button dLeft;
+    private final Button dRight;
+    private final Button dUp;
+    private final Button dDown;
+
+    private final Button a;
+    private final Button b;
+    private final Button x;
+    private final Button y;
+
+    private final Button leftBumper;
+    private final Button rightBumper;
+
+    private final Button back;
+    private final Button start;
+
+    private final Trigger leftTrigger;
+    private final Trigger rightTrigger;
+
+    private final Button touchpad;
+
+    private final Button allButtons;
+
+    public Controller(Gamepad gamepad) {
+        this.gamepad = gamepad;
+
+        leftStick = new Joystick();
+        rightStick = new Joystick();
+
+        leftStickButton = new Button();
+        rightStickButton = new Button();
+
+        dLeft = new Button();
+        dRight = new Button();
+        dUp = new Button();
+        dDown = new Button();
+
+        a = new Button();
+        b = new Button();
+        x = new Button();
+        y = new Button();
+
+        leftBumper = new Button();
+        rightBumper = new Button();
+
+        back = new Button();
+        start = new Button();
+
+        leftTrigger = new Trigger();
+        rightTrigger = new Trigger();
+
+        touchpad = new Button();
+
+        allButtons = new Button();
+    }
+
+
+    public void update() {
+        leftStick.update(gamepad.left_stick_x, -gamepad.left_stick_y);
+        rightStick.update(gamepad.right_stick_x, -gamepad.right_stick_y);
+
+        leftStickButton.update(gamepad.left_stick_button);
+        rightStickButton.update(gamepad.right_stick_button);
+
+        dLeft.update(gamepad.dpad_left);
+        dRight.update(gamepad.dpad_right);
+        dUp.update(gamepad.dpad_up);
+        dDown.update(gamepad.dpad_down);
+
+        a.update(gamepad.a);
+        b.update(gamepad.b);
+        x.update(gamepad.x);
+        y.update(gamepad.y);
+
+        leftBumper.update(gamepad.left_bumper);
+        rightBumper.update(gamepad.right_bumper);
+
+        back.update(gamepad.back);
+        start.update(gamepad.start);
+
+        leftTrigger.update(gamepad.left_trigger);
+        rightTrigger.update(gamepad.right_trigger);
+
+        touchpad.update(gamepad.touchpad);
+
+        allButtons.update(leftStickButton.isPressed() || rightStickButton.isPressed() || dLeft.isPressed() || dRight.isPressed() || dUp.isPressed() || dDown.isPressed() || a.isPressed() || b.isPressed() || x.isPressed() || y.isPressed() || leftBumper.isPressed() || rightBumper.isPressed() || back.isPressed() || start.isPressed() || leftTrigger.getValue() > 0 || rightTrigger.getValue() > 0 || touchpad.isPressed());
+    }
+
+    public Joystick getLeftStick() {
+        return leftStick;
+    }
+    public Joystick getRightStick() {
+        return rightStick;
+    }
+
+    public Button getLeftStickButton() {
+        return leftStickButton;
+    }
+    public Button getRightStickButton() {
+        return rightStickButton;
+    }
+
+    public Button getDLeft() {
+        return dLeft;
+    }
+    public Button getDRight() {
+        return dRight;
+    }
+    public Button getDUp() {
+        return dUp;
+    }
+    public Button getDDown() {
+        return dDown;
+    }
+
+    public Button getA() {
+        return a;
+    }
+    public Button getB() {
+        return b;
+    }
+    public Button getX() {
+        return x;
+    }
+    public Button getY() {
+        return y;
+    }
+
+    public Button getLeftBumper() {
+        return leftBumper;
+    }
+    public Button getRightBumper() {
+        return rightBumper;
+    }
+
+    public Button getBack() {
+        return back;
+    }
+    public Button getStart() {
+        return start;
+    }
+
+    public Trigger getLeftTrigger() {
+        return leftTrigger;
+    }
+    public Trigger getRightTrigger() {
+        return rightTrigger;
+    }
+
+    public Button getTouchpad() {
+        return touchpad;
+    }
+
+    public Button getAllButtons() {
+        return allButtons;
+    }
+
+    public void rumble() {
+        gamepad.rumble(Gamepad.RUMBLE_DURATION_CONTINUOUS);
+    }
+
+    public void rumble(int milliseconds) {
+        gamepad.rumble(milliseconds);
+    }
+
+    public void rumbleBlips(int blips) {
+        gamepad.rumbleBlips(blips);
+    }
+
+    public void stopRumble() {
+        gamepad.stopRumble();
+    }
+
+    public boolean isRumbling() {
+        return gamepad.isRumbling();
+    }
+
+    public void setColor(int r, int g, int b) {
+        gamepad.setLedColor(r, g, b, -1);
+    }
+    public void setColor(int r, int g, int b, int milliseconds) {
+        gamepad.setLedColor(r, g, b, milliseconds);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Gamepad2.java

@@ -0,0 +1,913 @@
+package org.firstinspires.ftc.teamcode.controller;///*
+// * Copyright (c) 2014, 2015 Qualcomm Technologies Inc
+// *
+// * 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 Qualcomm Technologies Inc 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 com.qualcomm.robotcore.hardware;
+//
+//import android.os.SystemClock;
+//
+//import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
+//import com.qualcomm.robotcore.exception.RobotCoreException;
+//import com.qualcomm.robotcore.robocol.RobocolParsable;
+//import com.qualcomm.robotcore.robocol.RobocolParsableBase;
+//import com.qualcomm.robotcore.util.Range;
+//import com.qualcomm.robotcore.util.RobotLog;
+//
+//import org.firstinspires.ftc.robotcore.internal.collections.EvictingBlockingQueue;
+//import org.firstinspires.ftc.robotcore.internal.collections.SimpleGson;
+//import org.firstinspires.ftc.robotcore.internal.network.SendOnceRunnable;
+//import org.firstinspires.ftc.robotcore.internal.ui.GamepadUser;
+//
+//import java.nio.BufferOverflowException;
+//import java.nio.ByteBuffer;
+//import java.util.ArrayList;
+//import java.util.concurrent.ArrayBlockingQueue;
+//
+///**
+// * Monitor a hardware gamepad.
+// * <p>
+// * The buttons, analog sticks, and triggers are represented a public
+// * member variables that can be read from or written to directly.
+// * <p>
+// * Analog sticks are represented as floats that range from -1.0 to +1.0. They will be 0.0 while at
+// * rest. The horizontal axis is labeled x, and the vertical axis is labeled y.
+// * <p>
+// * Triggers are represented as floats that range from 0.0 to 1.0. They will be at 0.0 while at
+// * rest.
+// * <p>
+// * Buttons are boolean values. They will be true if the button is pressed, otherwise they will be
+// * false.
+// * <p>
+// * The codes KEYCODE_BUTTON_SELECT and KEYCODE_BACK are both be handled as a "back" button event.
+// * Older Android devices (Kit Kat) map a Logitech F310 "back" button press to a KEYCODE_BUTTON_SELECT event.
+// * Newer Android devices (Marshmallow or greater) map this "back" button press to a KEYCODE_BACK event.
+// * Also, the REV Robotics Gamepad (REV-31-1159) has a "select" button instead of a "back" button on the gamepad.
+// * <p>
+// * The dpad is represented as 4 buttons, dpad_up, dpad_down, dpad_left, and dpad_right
+// */
+//@SuppressWarnings("unused")
+//public class Gamepad2 extends RobocolParsableBase {
+//
+//    /**
+//     * A gamepad with an ID equal to ID_UNASSOCIATED has not been associated with any device.
+//     */
+//    public static final int ID_UNASSOCIATED = -1;
+//
+//    /**
+//     * A gamepad with a phantom id a synthetic one made up by the system
+//     */
+//    public static final int ID_SYNTHETIC = -2;
+//
+//    public enum Type {
+//        // Do NOT change the order/names of existing entries,
+//        // you will break backwards compatibility!!
+//        UNKNOWN(LegacyType.UNKNOWN),
+//        LOGITECH_F310(LegacyType.LOGITECH_F310),
+//        XBOX_360(LegacyType.XBOX_360),
+//        SONY_PS4(LegacyType.SONY_PS4), // This indicates a PS4-compatible controller that is being used through our compatibility mode
+//        SONY_PS4_SUPPORTED_BY_KERNEL(LegacyType.SONY_PS4); // This indicates a PS4-compatible controller that is being used through the DualShock 4 Linux kernel driver.
+//
+//        private final LegacyType correspondingLegacyType;
+//        Type(LegacyType correspondingLegacyType) {
+//            this.correspondingLegacyType = correspondingLegacyType;
+//        }
+//    }
+//
+//    // LegacyType is necessary because robocol gamepad version 3 was written in a way that was not
+//    // forwards-compatible, so we have to keep sending V3-compatible values.
+//    public enum LegacyType {
+//        // Do NOT change the order or names of existing entries, or add new entries.
+//        // You will break backwards compatibility!!
+//        UNKNOWN,
+//        LOGITECH_F310,
+//        XBOX_360,
+//        SONY_PS4;
+//    }
+//
+//    @SuppressWarnings("UnusedAssignment")
+//    public volatile Type type = Type.UNKNOWN; // IntelliJ thinks this is redundant, but it is NOT. Must be a bug in the analyzer?
+//
+//    /**
+//     * left analog stick horizontal axis
+//     */
+//    public volatile float left_stick_x = 0f;
+//
+//    /**
+//     * left analog stick vertical axis
+//     */
+//    public volatile float left_stick_y = 0f;
+//
+//    /**
+//     * right analog stick horizontal axis
+//     */
+//    public volatile float right_stick_x = 0f;
+//
+//    /**
+//     * right analog stick vertical axis
+//     */
+//    public volatile float right_stick_y = 0f;
+//
+//    /**
+//     * dpad up
+//     */
+//    public volatile boolean dpad_up = false;
+//
+//    /**
+//     * dpad down
+//     */
+//    public volatile boolean dpad_down = false;
+//
+//    /**
+//     * dpad left
+//     */
+//    public volatile boolean dpad_left = false;
+//
+//    /**
+//     * dpad right
+//     */
+//    public volatile boolean dpad_right = false;
+//
+//    /**
+//     * button a
+//     */
+//    public volatile boolean a = false;
+//
+//    /**
+//     * button b
+//     */
+//    public volatile boolean b = false;
+//
+//    /**
+//     * button x
+//     */
+//    public volatile boolean x = false;
+//
+//    /**
+//     * button y
+//     */
+//    public volatile boolean y = false;
+//
+//    /**
+//     * button guide - often the large button in the middle of the controller. The OS may
+//     * capture this button before it is sent to the app; in which case you'll never
+//     * receive it.
+//     */
+//    public volatile boolean guide = false;
+//
+//    /**
+//     * button start
+//     */
+//    public volatile boolean start = false;
+//
+//    /**
+//     * button back
+//     */
+//    public volatile boolean back = false;
+//
+//    /**
+//     * button left bumper
+//     */
+//    public volatile boolean left_bumper = false;
+//
+//    /**
+//     * button right bumper
+//     */
+//    public volatile boolean right_bumper = false;
+//
+//    /**
+//     * left stick button
+//     */
+//    public volatile boolean left_stick_button = false;
+//
+//    /**
+//     * right stick button
+//     */
+//    public volatile boolean right_stick_button = false;
+//
+//    /**
+//     * left trigger
+//     */
+//    public volatile float left_trigger = 0f;
+//
+//    /**
+//     * right trigger
+//     */
+//    public volatile float right_trigger = 0f;
+//
+//    /**
+//     * PS4 Support - Circle
+//     */
+//    public volatile boolean circle = false;
+//
+//    /**
+//     * PS4 Support - cross
+//     */
+//    public volatile boolean cross = false;
+//
+//    /**
+//     * PS4 Support - triangle
+//     */
+//    public volatile boolean triangle = false;
+//
+//    /**
+//     * PS4 Support - square
+//     */
+//    public volatile boolean square = false;
+//
+//    /**
+//     * PS4 Support - share
+//     */
+//    public volatile boolean share = false;
+//
+//    /**
+//     * PS4 Support - options
+//     */
+//    public volatile boolean options = false;
+//
+//    /**
+//     * PS4 Support - touchpad
+//     */
+//    public volatile boolean touchpad = false;
+//    public volatile boolean touchpad_finger_1;
+//    public volatile boolean touchpad_finger_2;
+//    public volatile float touchpad_finger_1_x;
+//    public volatile float touchpad_finger_1_y;
+//    public volatile float touchpad_finger_2_x;
+//    public volatile float touchpad_finger_2_y;
+//
+//    /**
+//     * PS4 Support - PS Button
+//     */
+//    public volatile boolean ps = false;
+//
+//    /**
+//     * Which user is this gamepad used by
+//     */
+//    protected volatile byte user = ID_UNASSOCIATED;
+//    //
+//    public GamepadUser getUser() {
+//        return GamepadUser.from(user);
+//    }
+//    //
+//    public void setUser(GamepadUser user) {
+//        this.user = user.id;
+//    }
+//
+//    /**
+//     * See {@link OpModeManagerImpl#runActiveOpMode(Gamepad[])}
+//     */
+//    protected volatile byte userForEffects = ID_UNASSOCIATED;
+//    public void setUserForEffects(byte userForEffects) {
+//        this.userForEffects = userForEffects;
+//    }
+//
+//    /**
+//     * ID assigned to this gamepad by the OS. This value can change each time the device is plugged in.
+//     */
+//    public volatile int id = ID_UNASSOCIATED;  // public only for historical reasons
+//
+//    public void setGamepadId(int id) {
+//        this.id = id;
+//    }
+//    public int getGamepadId() {
+//        return this.id;
+//    }
+//
+//    /**
+//     * Relative timestamp of the last time an event was detected
+//     */
+//    public volatile long timestamp = 0;
+//
+//    /**
+//     * Sets the time at which this Gamepad last changed its state,
+//     * in the {@link android.os.SystemClock#uptimeMillis} time base.
+//     */
+//    public void setTimestamp(long timestamp) {
+//        this.timestamp = timestamp;
+//    }
+//
+//    /**
+//     * Refreshes the Gamepad's timestamp to be the current time.
+//     */
+//    public void refreshTimestamp() {
+//        setTimestamp(SystemClock.uptimeMillis());
+//    }
+//
+//    // private static values used for packaging the gamepad state into a byte array
+//    private static final short PAYLOAD_SIZE = 60;
+//    private static final short BUFFER_SIZE = PAYLOAD_SIZE + RobocolParsable.HEADER_LENGTH;
+//
+//    private static final byte ROBOCOL_GAMEPAD_VERSION = 5;
+//
+//    public Gamepad() {
+//        this.type = type();
+//    }
+//
+//    /**
+//     * Copy the state of a gamepad into this gamepad
+//     * @param gamepad state to be copied from
+//     * @throws RobotCoreException if the copy fails - gamepad will be in an unknown
+//     *         state if this exception is thrown
+//     */
+//    public void copy(Gamepad gamepad) throws RobotCoreException {
+//        // reuse the serialization code; since that reduces the chances of bugs
+//        fromByteArray(gamepad.toByteArray());
+//    }
+//
+//    /**
+//     * Reset this gamepad into its initial state
+//     */
+//    public void reset() {
+//        try {
+//            copy(new Gamepad());
+//        } catch (RobotCoreException e) {
+//            // we should never hit this
+//            RobotLog.e("Gamepad library in an invalid state");
+//            throw new IllegalStateException("Gamepad library in an invalid state");
+//        }
+//    }
+//
+//    @Override
+//    public MsgType getRobocolMsgType() {
+//        return RobocolParsable.MsgType.GAMEPAD;
+//    }
+//
+//    @Override
+//    public byte[] toByteArray() throws RobotCoreException {
+//
+//        ByteBuffer buffer = getWriteBuffer(PAYLOAD_SIZE);
+//
+//        try {
+//            int buttons = 0;
+//
+//            buffer.put(ROBOCOL_GAMEPAD_VERSION);
+//            buffer.putInt(id);
+//            buffer.putLong(timestamp).array();
+//            buffer.putFloat(left_stick_x).array();
+//            buffer.putFloat(left_stick_y).array();
+//            buffer.putFloat(right_stick_x).array();
+//            buffer.putFloat(right_stick_y).array();
+//            buffer.putFloat(left_trigger).array();
+//            buffer.putFloat(right_trigger).array();
+//
+//            buttons = (buttons << 1) + (touchpad_finger_1 ? 1 : 0);
+//            buttons = (buttons << 1) + (touchpad_finger_2 ? 1 : 0);
+//            buttons = (buttons << 1) + (touchpad ? 1 : 0);
+//            buttons = (buttons << 1) + (left_stick_button ? 1 : 0);
+//            buttons = (buttons << 1) + (right_stick_button ? 1 : 0);
+//            buttons = (buttons << 1) + (dpad_up ? 1 : 0);
+//            buttons = (buttons << 1) + (dpad_down ? 1 : 0);
+//            buttons = (buttons << 1) + (dpad_left ? 1 : 0);
+//            buttons = (buttons << 1) + (dpad_right ? 1 : 0);
+//            buttons = (buttons << 1) + (a ? 1 : 0);
+//            buttons = (buttons << 1) + (b ? 1 : 0);
+//            buttons = (buttons << 1) + (x ? 1 : 0);
+//            buttons = (buttons << 1) + (y ? 1 : 0);
+//            buttons = (buttons << 1) + (guide ? 1 : 0);
+//            buttons = (buttons << 1) + (start ? 1 : 0);
+//            buttons = (buttons << 1) + (back ? 1 : 0);
+//            buttons = (buttons << 1) + (left_bumper ? 1 : 0);
+//            buttons = (buttons << 1) + (right_bumper ? 1 : 0);
+//            buffer.putInt(buttons);
+//
+//            // Version 2
+//            buffer.put(user);
+//
+//            // Version 3
+//            buffer.put((byte) legacyType().ordinal());
+//
+//            // Version 4
+//            buffer.put((byte) type.ordinal());
+//
+//            // Version 5
+//            buffer.putFloat(touchpad_finger_1_x);
+//            buffer.putFloat(touchpad_finger_1_y);
+//            buffer.putFloat(touchpad_finger_2_x);
+//            buffer.putFloat(touchpad_finger_2_y);
+//        } catch (BufferOverflowException e) {
+//            RobotLog.logStacktrace(e);
+//        }
+//
+//        return buffer.array();
+//    }
+//
+//    @Override
+//    public void fromByteArray(byte[] byteArray) throws RobotCoreException {
+//        if (byteArray.length < BUFFER_SIZE) {
+//            throw new RobotCoreException("Expected buffer of at least " + BUFFER_SIZE + " bytes, received " + byteArray.length);
+//        }
+//
+//        ByteBuffer byteBuffer = getReadBuffer(byteArray);
+//
+//        int buttons = 0;
+//
+//        byte version = byteBuffer.get();
+//
+//        // TODO(Noah): Reset version to 1
+//        // extract version 1 values
+//        if (version >= 1) {
+//            id = byteBuffer.getInt();
+//            timestamp = byteBuffer.getLong();
+//            left_stick_x = byteBuffer.getFloat();
+//            left_stick_y = byteBuffer.getFloat();
+//            right_stick_x = byteBuffer.getFloat();
+//            right_stick_y = byteBuffer.getFloat();
+//            left_trigger = byteBuffer.getFloat();
+//            right_trigger = byteBuffer.getFloat();
+//
+//            buttons = byteBuffer.getInt();
+//            touchpad_finger_1   = (buttons & 0x20000) != 0;
+//            touchpad_finger_2   = (buttons & 0x10000) != 0;
+//            touchpad            = (buttons & 0x08000) != 0;
+//            left_stick_button   = (buttons & 0x04000) != 0;
+//            right_stick_button  = (buttons & 0x02000) != 0;
+//            dpad_up             = (buttons & 0x01000) != 0;
+//            dpad_down           = (buttons & 0x00800) != 0;
+//            dpad_left           = (buttons & 0x00400) != 0;
+//            dpad_right          = (buttons & 0x00200) != 0;
+//            a                   = (buttons & 0x00100) != 0;
+//            b                   = (buttons & 0x00080) != 0;
+//            x                   = (buttons & 0x00040) != 0;
+//            y                   = (buttons & 0x00020) != 0;
+//            guide               = (buttons & 0x00010) != 0;
+//            start               = (buttons & 0x00008) != 0;
+//            back                = (buttons & 0x00004) != 0;
+//            left_bumper         = (buttons & 0x00002) != 0;
+//            right_bumper        = (buttons & 0x00001) != 0;
+//        }
+//
+//        // extract version 2 values
+//        if (version >= 2) {
+//            user = byteBuffer.get();
+//        }
+//
+//        // extract version 3 values
+//        if (version >= 3) {
+//            type = Type.values()[byteBuffer.get()];
+//        }
+//
+//        if (version >= 4) {
+//            byte v4TypeValue = byteBuffer.get();
+//            if (v4TypeValue < Type.values().length) {
+//                // Yes, this will replace the version 3 value. That is a good thing, since the version 3
+//                // value was not forwards-compatible.
+//                type = Type.values()[v4TypeValue];
+//            } // Else, we don't know what the number means, so we just stick with the value we got from the v3 type field
+//        }
+//
+//        if(version >= 5) {
+//            touchpad_finger_1_x = byteBuffer.getFloat();
+//            touchpad_finger_1_y = byteBuffer.getFloat();
+//            touchpad_finger_2_x = byteBuffer.getFloat();
+//            touchpad_finger_2_y = byteBuffer.getFloat();
+//        }
+//
+//        updateButtonAliases();
+//    }
+//
+//    /**
+//     * Are all analog sticks and triggers in their rest position?
+//     * @return true if all analog sticks and triggers are at rest; otherwise false
+//     */
+//    public boolean atRest() {
+//        return (
+//                left_stick_x == 0f && left_stick_y == 0f &&
+//                        right_stick_x == 0f && right_stick_y == 0f &&
+//                        left_trigger == 0f && right_trigger == 0f);
+//    }
+//
+//    /**
+//     * Get the type of gamepad as a {@link Type}. This method defaults to "UNKNOWN".
+//     * @return gamepad type
+//     */
+//    public Type type() {
+//        return type;
+//    }
+//
+//    /**
+//     * Get the type of gamepad as a {@link LegacyType}. This method defaults to "UNKNOWN".
+//     * @return gamepad type
+//     */
+//    private LegacyType legacyType() {
+//        return type.correspondingLegacyType;
+//    }
+//
+//
+//    /**
+//     * Display a summary of this gamepad, including the state of all buttons, analog sticks, and triggers
+//     * @return a summary
+//     */
+//    @Override
+//    public String toString() {
+//
+//        switch (type) {
+//            case SONY_PS4:
+//            case SONY_PS4_SUPPORTED_BY_KERNEL:
+//                return ps4ToString();
+//
+//            case UNKNOWN:
+//            case LOGITECH_F310:
+//            case XBOX_360:
+//            default:
+//                return genericToString();
+//        }
+//    }
+//
+//
+//    protected String ps4ToString() {
+//        String buttons = new String();
+//        if (dpad_up) buttons += "dpad_up ";
+//        if (dpad_down) buttons += "dpad_down ";
+//        if (dpad_left) buttons += "dpad_left ";
+//        if (dpad_right) buttons += "dpad_right ";
+//        if (cross) buttons += "cross ";
+//        if (circle) buttons += "circle ";
+//        if (square) buttons += "square ";
+//        if (triangle) buttons += "triangle ";
+//        if (ps) buttons += "ps ";
+//        if (share) buttons += "share ";
+//        if (options) buttons += "options ";
+//        if (touchpad) buttons += "touchpad ";
+//        if (left_bumper) buttons += "left_bumper ";
+//        if (right_bumper) buttons += "right_bumper ";
+//        if (left_stick_button) buttons += "left stick button ";
+//        if (right_stick_button) buttons += "right stick button ";
+//
+//        return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s",
+//                id, user, left_stick_x, left_stick_y,
+//                right_stick_x, right_stick_y, left_trigger, right_trigger, buttons);
+//    }
+//
+//    protected String genericToString() {
+//        String buttons = new String();
+//        if (dpad_up) buttons += "dpad_up ";
+//        if (dpad_down) buttons += "dpad_down ";
+//        if (dpad_left) buttons += "dpad_left ";
+//        if (dpad_right) buttons += "dpad_right ";
+//        if (a) buttons += "a ";
+//        if (b) buttons += "b ";
+//        if (x) buttons += "x ";
+//        if (y) buttons += "y ";
+//        if (guide) buttons += "guide ";
+//        if (start) buttons += "start ";
+//        if (back) buttons += "back ";
+//        if (left_bumper) buttons += "left_bumper ";
+//        if (right_bumper) buttons += "right_bumper ";
+//        if (left_stick_button) buttons += "left stick button ";
+//        if (right_stick_button) buttons += "right stick button ";
+//
+//        return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s",
+//                id, user, left_stick_x, left_stick_y,
+//                right_stick_x, right_stick_y, left_trigger, right_trigger, buttons);
+//    }
+//
+//
+//    // To prevent blowing up the command queue if the user tries to send an LED command in a tight loop,
+//    // we have a 1-element evicting blocking queue for the outgoing LED effect and the event loop periodically
+//    // just grabs the effect out of it (if any)
+//    public EvictingBlockingQueue<LedEffect> ledQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue<LedEffect>(1));
+//
+//    public static final int LED_DURATION_CONTINUOUS = -1;
+//
+//    public static class LedEffect {
+//        public static class Step {
+//            public int r;
+//            public int g;
+//            public int b;
+//            public int duration;
+//        }
+//
+//        public final ArrayList<Step> steps;
+//        public final boolean repeating;
+//        public int user;
+//
+//        private LedEffect(ArrayList<Step> steps, boolean repeating) {
+//            this.steps = steps;
+//            this.repeating = repeating;
+//        }
+//
+//        public String serialize() {
+//            return SimpleGson.getInstance().toJson(this);
+//        }
+//
+//        public static LedEffect deserialize(String serialized) {
+//            return SimpleGson.getInstance().fromJson(serialized, LedEffect.class);
+//        }
+//
+//        public static class Builder {
+//            private ArrayList<Step> steps = new ArrayList<>();
+//            private boolean repeating;
+//
+//            /**
+//             * Add a "step" to this LED effect. A step basically just means to set
+//             * the LED to a certain color (r,g,b) for a certain duration. By creating a chain of
+//             * steps, you can create unique effects.
+//             *
+//             * @param r the red LED intensity (0.0 - 1.0)
+//             * @param g the green LED intensity (0.0 - 1.0)
+//             * @param b the blue LED intensity (0.0 - 1.0)
+//             * @return the builder object, to follow the builder pattern
+//             */
+//            public Builder addStep(double r, double g, double b, int durationMs) {
+//                return addStepInternal(r, g, b, Math.max(durationMs, 0));
+//            }
+//
+//            /**
+//             * Set whether this LED effect should loop after finishing,
+//             * unless the LED is otherwise commanded differently.
+//             * @param repeating whether to loop
+//             * @return the builder object, to follow the builder pattern
+//             */
+//            public Builder setRepeating(boolean repeating) {
+//                this.repeating = repeating;
+//                return this;
+//            }
+//
+//            private Builder addStepInternal(double r, double g, double b, int duration) {
+//
+//                r = Range.clip(r, 0, 1);
+//                g = Range.clip(g, 0, 1);
+//                b = Range.clip(b, 0, 1);
+//
+//                Step step = new Step();
+//                step.r = (int) Math.round(Range.scale(r, 0.0, 1.0, 0, 255));
+//                step.g = (int) Math.round(Range.scale(g, 0.0, 1.0, 0, 255));
+//                step.b = (int) Math.round(Range.scale(b, 0.0, 1.0, 0, 255));
+//                step.duration = duration;
+//                steps.add(step);
+//                return this;
+//            }
+//
+//            /**
+//             * After you've added your steps, call this to get an LedEffect object
+//             * that you can then pass to {@link #runLedEffect(LedEffect)}
+//             * @return an LedEffect object, built from previously added steps
+//             */
+//            public LedEffect build() {
+//                return new LedEffect(steps, repeating);
+//            }
+//        }
+//    }
+//
+//    public void setLedColor(double r, double g, double b, int durationMs) {
+//
+//        if (durationMs != LED_DURATION_CONTINUOUS) {
+//            durationMs = Math.max(0, durationMs);
+//        }
+//
+//        LedEffect effect = new LedEffect.Builder().addStepInternal(r,g,b, durationMs).build();
+//        queueEffect(effect);
+//    }
+//
+//    /**
+//     * Run an LED effect built using {@link LedEffect.Builder}
+//     * The LED effect will be run asynchronously; your OpMode will
+//     * not halt execution while the effect is running.
+//     *
+//     * Calling this will displace any currently running LED effect
+//     */
+//    public void runLedEffect(LedEffect effect) {
+//        queueEffect(effect);
+//    }
+//
+//    private void queueEffect(LedEffect effect) {
+//        // We need to make a new object here, because since the effect is queued and
+//        // not set immediately, if you called this function for two different gamepads
+//        // but passed in the same instance of an LED effect object, then it could happen
+//        // that both effect commands would be directed to the *same* gamepad. (Because of
+//        // the "user" field being modified before the queued command was serialized)
+//        LedEffect copy = new LedEffect(effect.steps, effect.repeating);
+//        copy.user = userForEffects;
+//        ledQueue.offer(copy);
+//    }
+//
+//    // To prevent blowing up the command queue if the user tries to send a rumble command in a tight loop,
+//    // we have a 1-element evicting blocking queue for the outgoing rumble effect and the event loop periodically
+//    // just grabs the effect out of it (if any)
+//    public EvictingBlockingQueue<RumbleEffect> rumbleQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue<RumbleEffect>(1));
+//    public long nextRumbleApproxFinishTime = RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING;
+//
+//    public static final int RUMBLE_DURATION_CONTINUOUS = -1;
+//
+//    public static class RumbleEffect {
+//        public static class Step {
+//            public int large;
+//            public int small;
+//            public int duration;
+//        }
+//
+//        public int user;
+//        public final ArrayList<Step> steps;
+//
+//        private RumbleEffect(ArrayList<Step> steps) {
+//            this.steps = steps;
+//        }
+//
+//        public String serialize() {
+//            return SimpleGson.getInstance().toJson(this);
+//        }
+//
+//        public static RumbleEffect deserialize(String serialized) {
+//            return SimpleGson.getInstance().fromJson(serialized, RumbleEffect.class);
+//        }
+//
+//        public static class Builder {
+//            private ArrayList<Step> steps = new ArrayList<>();
+//
+//            /**
+//             * Add a "step" to this rumble effect. A step basically just means to rumble
+//             * at a certain power level for a certain duration. By creating a chain of
+//             * steps, you can create unique effects. See {@link #rumbleBlips(int)} for a
+//             * a simple example.
+//             * @param rumble1 rumble power for rumble motor 1 (0.0 - 1.0)
+//             * @param rumble2 rumble power for rumble motor 2 (0.0 - 1.0)
+//             * @param durationMs milliseconds this step lasts
+//             * @return the builder object, to follow the builder pattern
+//             */
+//            public Builder addStep(double rumble1, double rumble2, int durationMs) {
+//                return addStepInternal(rumble1, rumble2, Math.max(durationMs, 0));
+//            }
+//
+//            private Builder addStepInternal(double rumble1, double rumble2, int durationMs) {
+//
+//                rumble1 = Range.clip(rumble1, 0, 1);
+//                rumble2 = Range.clip(rumble2, 0, 1);
+//
+//                Step step = new Step();
+//                step.large = (int) Math.round(Range.scale(rumble1, 0.0, 1.0, 0, 255));
+//                step.small = (int) Math.round(Range.scale(rumble2, 0.0, 1.0, 0, 255));
+//                step.duration = durationMs;
+//                steps.add(step);
+//
+//                return this;
+//            }
+//
+//            /**
+//             * After you've added your steps, call this to get a RumbleEffect object
+//             * that you can then pass to {@link #runRumbleEffect(RumbleEffect)}
+//             * @return a RumbleEffect object, built from previously added steps
+//             */
+//            public RumbleEffect build() {
+//                return new RumbleEffect(steps);
+//            }
+//        }
+//    }
+//
+//    /**
+//     * Run a rumble effect built using {@link RumbleEffect.Builder}
+//     * The rumble effect will be run asynchronously; your OpMode will
+//     * not halt execution while the effect is running.
+//     *
+//     * Calling this will displace any currently running rumble effect
+//     */
+//    public void runRumbleEffect(RumbleEffect effect) {
+//        queueEffect(effect);
+//    }
+//
+//    /**
+//     * Rumble the gamepad's first rumble motor at maximum power for a certain duration.
+//     * Calling this will displace any currently running rumble effect.
+//     * @param durationMs milliseconds to rumble for, or {@link #RUMBLE_DURATION_CONTINUOUS}
+//     */
+//    public void rumble(int durationMs) {
+//
+//        if (durationMs != RUMBLE_DURATION_CONTINUOUS) {
+//            durationMs = Math.max(0, durationMs);
+//        }
+//
+//        RumbleEffect effect = new RumbleEffect.Builder().addStepInternal(1.0, 0, durationMs).build();
+//        queueEffect(effect);
+//    }
+//
+//    /**
+//     * Rumble the gamepad at a fixed rumble power for a certain duration
+//     * Calling this will displace any currently running rumble effect
+//     * @param rumble1 rumble power for rumble motor 1 (0.0 - 1.0)
+//     * @param rumble2 rumble power for rumble motor 2 (0.0 - 1.0)
+//     * @param durationMs milliseconds to rumble for, or {@link #RUMBLE_DURATION_CONTINUOUS}
+//     */
+//    public void rumble(double rumble1, double rumble2, int durationMs) {
+//
+//        if (durationMs != RUMBLE_DURATION_CONTINUOUS) {
+//            durationMs = Math.max(0, durationMs);
+//        }
+//
+//        RumbleEffect effect = new RumbleEffect.Builder().addStepInternal(rumble1, rumble2, durationMs).build();
+//        queueEffect(effect);
+//    }
+//
+//    /**
+//     * Cancel the currently running rumble effect, if any
+//     */
+//    public void stopRumble() {
+//        rumble(0,0,RUMBLE_DURATION_CONTINUOUS);
+//    }
+//
+//    /**
+//     * Rumble the gamepad for a certain number of "blips" using predetermined blip timing
+//     * This will displace any currently running rumble effect.
+//     * @param count the number of rumble blips to perform
+//     */
+//    public void rumbleBlips(int count) {
+//        RumbleEffect.Builder builder = new RumbleEffect.Builder();
+//
+//        for(int i = 0; i < count; i++) {
+//            builder.addStep(1.0,0,50).addStep(0,0,50);
+//        }
+//
+//        queueEffect(builder.build());
+//    }
+//
+//    private void queueEffect(RumbleEffect effect) {
+//        // We need to make a new object here, because since the effect is queued and
+//        // not set immediately, if you called this function for two different gamepads
+//        // but passed in the same instance of a rumble effect object, then it could happen
+//        // that both rumble commands would be directed to the *same* gamepad. (Because of
+//        // the "user" field being modified before the queued command was serialized)
+//        RumbleEffect copy = new RumbleEffect(effect.steps);
+//        copy.user = userForEffects;
+//        rumbleQueue.offer(copy);
+//        nextRumbleApproxFinishTime = calcApproxRumbleFinishTime(copy);
+//    }
+//
+//    private static final long RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING = -1;
+//    private static final long RUMBLE_FINISH_TIME_FLAG_INFINITE = Long.MAX_VALUE;
+//
+//    /**
+//     * Returns an educated guess about whether there is a rumble action ongoing on this gamepad
+//     * @return an educated guess about whether there is a rumble action ongoing on this gamepad
+//     */
+//    public boolean isRumbling() {
+//        if(nextRumbleApproxFinishTime == RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING) {
+//            return false;
+//        } else if(nextRumbleApproxFinishTime == RUMBLE_FINISH_TIME_FLAG_INFINITE) {
+//            return true;
+//        } else {
+//            return System.currentTimeMillis() < nextRumbleApproxFinishTime;
+//        }
+//    }
+//
+//    private long calcApproxRumbleFinishTime(RumbleEffect effect) {
+//        // If the effect is only 1 step long and has an infinite duration...
+//        if(effect.steps.size() == 1 && effect.steps.get(0).duration == RUMBLE_DURATION_CONTINUOUS) {
+//            // If the power is zero, then that means the gamepad is being commanded to cease rumble
+//            if (effect.steps.get(0).large == 0 && effect.steps.get(0).small == 0) {
+//                return RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING;
+//            } else { // But if not, that means it's an infinite (continuous) rumble command
+//                return RUMBLE_FINISH_TIME_FLAG_INFINITE;
+//            }
+//        } else { // If the effect has more than one step (or one step with non-infinite duration) we need to sum the step times
+//            long time = System.currentTimeMillis();
+//            long overhead = 50 /* rumbleGamepadsInterval in FtcEventLoopHandler */ +
+//                    SendOnceRunnable.MS_BATCH_TRANSMISSION_INTERVAL +
+//                    5  /* Slop */;
+//            for(RumbleEffect.Step step : effect.steps) {
+//                time += step.duration;
+//            }
+//            time += overhead;
+//            return time;
+//        }
+//    }
+//
+//    /**
+//     * Alias buttons so that XBOX &amp; PS4 native button labels can be used in use code.
+//     * Should allow a team to program with whatever controllers they prefer, but
+//     * be able to swap controllers easily without changing code.
+//     */
+//    protected void updateButtonAliases(){
+//        // There is no assignment for touchpad because there is no equivalent on XBOX controllers.
+//        circle = b;
+//        cross = a;
+//        triangle = y;
+//        square = x;
+//        share = back;
+//        options = start;
+//        ps = guide;
+//    }
+//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Joystick.java

@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.controller;
+
+public class Joystick {
+    public static double DEADZONE = 0.05; //small 5% deadzone for the new controlers
+    double x = 0;
+    double y = 0;
+
+    public Joystick() {}
+
+    public void update(double x, double y) {
+        this.x = x;
+        this.y = y;
+    }
+
+    public double getX() {
+        if(x<DEADZONE && x>-DEADZONE) {
+            return 0;
+        }
+        else{
+            return x;
+        }
+    }
+
+    public double getY() {
+        if(y<DEADZONE && y>-DEADZONE) {
+            return 0;
+        }
+        else{
+            return y;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Trigger.java

@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode.controller;
+
+public class Trigger {
+    double value = 0;
+    double currentState;
+    double lastState;
+    boolean justPressed = false;
+    boolean justReleased = false;
+
+    public Trigger() {}
+
+    public void update(double value) {
+        lastState = currentState;
+        currentState = value;
+
+        justPressed = currentState != 0 && lastState == 0;
+        justReleased = currentState == 0 && lastState != 0;
+
+        this.value = value;
+    }
+
+    public double getValue() {
+        return value;
+    }
+
+    public boolean isJustPressed() {
+        return justPressed;
+    }
+
+    public boolean isJustReleased() {
+        return justReleased;
+    }
+}

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

@@ -0,0 +1,171 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PController;
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Config
+public class Arm {
+    //make each servo
+    private Servo rAServo;
+    private Servo lAServo;
+    private Servo doorServo;
+    private Servo wristServo;
+
+    private Servo elbow;
+
+
+
+    private Slides.Position pos = Slides.Position.DOWN;
+    private PController armController;
+    private double armControllerTarget;
+
+    public static double armTolerance = 0.03;
+    public static double armSpeed = 0.022;
+    private double armPos;
+    private double armTempPos;
+    private double doorPos;
+    private double wristPos;
+
+    private double elbowPos;
+    public static double ARM_KP = 0.18;
+
+    public static double doorOpenPos = 0.36;
+    public static double doorClosePos = 0.61;
+
+    public static double armStart = 0.24;
+    public static double armScore = 0.95;
+
+    public static double armScore_highMacro = 0.95;
+    public static double wristStart = 0.8;
+    public static double wristScore = 0.31;
+
+    public static double elbow_mid = 0.5;
+    public static double elbow_right = 0.29;
+    public static double elbow_left = 0.95;
+
+    public static double wristScore_highMacro = 0.3;
+
+    public double counter;
+
+    public enum Position {
+        INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT
+    }
+
+    public enum DoorPosition {
+        OPEN, CLOSE
+    }
+
+    public enum Elbowpos{
+
+    }
+
+    public Arm(HardwareMap hardwareMap) {
+        wristServo = hardwareMap.get(Servo.class, "Wrist");
+        doorServo = hardwareMap.get(Servo.class, "Door");
+        lAServo = hardwareMap.get(Servo.class, "LeftArm");
+        rAServo = hardwareMap.get(Servo.class, "RightArm");
+        elbow = hardwareMap.get(Servo.class, "Elbow");
+//        lAServo.setDirection(Servo.Direction.REVERSE);
+        rAServo.setDirection(Servo.Direction.REVERSE);
+        doorServo.setDirection(Servo.Direction.REVERSE);
+//        wristServo.setDirection(Servo.Direction.REVERSE);
+
+        this.armController = new PController(ARM_KP);
+
+        setArmPos(Position.INTAKE);
+        lAServo.setPosition(armStart);
+        rAServo.setPosition(armStart);
+//        armTempPos = armPos;
+
+        setWristPos(Position.INTAKE);
+        setDoor(DoorPosition.CLOSE);
+        setElbowPos(1);
+    }
+
+    public void setArmPos(Position pos) {
+        this.armControllerTarget = pos == Position.INTAKE
+                ? armStart
+                : armScore;
+//        if (pos == Position.INTAKE) {
+//            armPos = armStart;
+//        } else if (pos == Position.SCORE) {
+//            armPos = armScore;
+//        }
+    }
+    public boolean armAtPosition() {
+        return armController.atSetPoint();
+//        return armTempPos == armPos;
+    }
+
+    public void setWristPos(Position pos) {
+        if (pos == Position.INTAKE) {
+            wristPos = wristStart;
+        } else if (pos == Position.SCORE) {
+            wristPos = wristScore;
+        } else if (pos == Position.SCOREHI) {
+            wristPos = wristScore_highMacro;
+
+        }
+    }
+
+    public void setElbowPos (int counter){
+        if (counter == 1) {
+            elbowPos = elbow_mid;
+        } else if (counter == 2) {
+            elbowPos = elbow_mid;
+        } else if (counter == 3) {
+            elbowPos = elbow_left;
+        } else if (counter == 4) {
+            elbowPos = elbow_right;
+        }
+    }
+
+    public void setDoor(DoorPosition pos) {
+        if (pos == DoorPosition.OPEN) {
+            doorPos = doorOpenPos;
+        } else if (pos == DoorPosition.CLOSE) {
+            doorPos = doorClosePos;
+        }
+    }
+
+    public String getTelemetry() {
+        return String.format("Wrist: %s\nDoor: %s\nLeft Arm: %s\nRight Arm: %s\nError: %s",
+                wristServo.getPosition(), doorServo.getPosition(), lAServo.getPosition(), rAServo.getPosition(), armController.getPositionError());
+        //return "Wrist Servo: "+wristServo.getPosition()+"\nLeft Arm Servo: "+lAServo.getPosition()+"\nRight Arm Servo: "+rAServo.getPosition()+"\nDoor Servo: "+ doorServo.getPosition();
+    }
+
+    public void update() {
+        this.armController.setSetPoint(this.armControllerTarget);
+        this.armController.setTolerance(armTolerance);
+        this.armController.setP(ARM_KP);
+
+        double output = 0;
+        if (!this.armController.atSetPoint()) {
+            output = Math.max(-1 * armScore, Math.min(armScore, this.armController.calculate(lAServo.getPosition())));
+
+            this.lAServo.setPosition(this.lAServo.getPosition() + output);
+            this.rAServo.setPosition(this.lAServo.getPosition() + output);
+        } else {
+            lAServo.setPosition(armControllerTarget);
+            rAServo.setPosition(armControllerTarget);
+        }
+//        if (Math.abs(armTempPos-armPos) <= armTolerance) {
+//            armTempPos = armPos;
+//        } else {
+//            if (armTempPos < armPos) {
+//                armTempPos += armSpeed;
+//            } else if (armTempPos > armPos) {
+//                armTempPos -= armSpeed;
+//            }
+//        }
+//        lAServo.setPosition(armTempPos);
+//        rAServo.setPosition(armTempPos);
+
+        doorServo.setPosition(doorPos);
+        wristServo.setPosition(wristPos);
+        elbow.setPosition(elbowPos);
+    }
+}

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

@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.tearabite.ielib.localization.AprilTagPoseEstimator;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+import org.firstinspires.ftc.teamcode.vision.Detection;
+import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+@Config
+public class Camera {
+    private static final String REAR_WEBCAM_NAME = "rearWebcam";
+    public static final String FRONT_WEBCAM_NAME = "frontWebcam";
+    private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(6.5, 0, Math.PI);
+    public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
+    public static float PROP_REJECTION_VERTICAL_LOWER = 440;
+    private PropDetectionPipeline prop;
+    private AprilTagProcessor aprilTag;
+    private VisionPortal aprilTagPortal;
+    private VisionPortal propPortal;
+    private boolean initialized;
+    private AprilTagPoseEstimator aprilTagPoseEstimator;
+
+    public Camera(HardwareMap hardwareMap) {
+        this.init(hardwareMap);
+    }
+
+    private void init(HardwareMap hardwareMap) {
+        this.aprilTagPoseEstimator = AprilTagPoseEstimator.builder()
+                .robotOffset(REAR_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, REAR_WEBCAM_NAME));
+        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, FRONT_WEBCAM_NAME));
+        propVisionPortalBuilder.setLiveViewContainerId(viewportIds[1]);
+        propVisionPortalBuilder.setAutoStopLiveView(true);
+        propVisionPortalBuilder.addProcessor(prop);
+        this.propPortal = propVisionPortalBuilder.build();
+
+        this.propPortal.resumeLiveView();
+        this.aprilTagPortal.resumeLiveView();
+        this.initialized = true;
+    }
+
+    public Detection getProp() {
+        if (!initialized || getAlliance() == null) {
+            return INVALID_DETECTION;
+        }
+
+        switch (getAlliance()) {
+            case Blue:
+                Detection blue = this.prop.getBlue();
+                return blue != null && blue.isValid() ? blue : INVALID_DETECTION;
+            case Red:
+                Detection red = this.prop.getRed();
+                return red != null && red.isValid() ? red : INVALID_DETECTION;
+        }
+
+        return INVALID_DETECTION;
+    }
+
+    public void setAlliance(CenterStageCommon.Alliance alliance) {
+        this.prop.setAlliance(alliance);
+    }
+
+    public CenterStageCommon.Alliance getAlliance() {
+        return this.prop != null ? this.prop.getAlliance() : null;
+    }
+
+    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);
+    }
+}

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

@@ -0,0 +1,120 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Config
+public class Intake {
+    private Servo rServo;
+    private Servo lServo;
+    private CRServo WheelServo;
+
+    private DcMotor dcMotor;
+    private Position pos = Position.UP;
+
+    public enum Position {
+        STACK1, STACK2, STACK3, STACK4, STACK5, STACK6, UP;
+
+        public Position nextPosition() {
+            int nextOne = (this.ordinal() + 1) % Position.values().length;
+            return Position.values()[nextOne];
+        }
+
+        public Position previousPosition() {
+            int backOne =Math.max(0,(this.ordinal() - 1) % Position.values().length);
+            return Position.values()[backOne];
+        }
+    }
+
+    //Position
+    public static double stack1 = 0.015;
+    public static double stack2 = 0.029;
+    public static double stack3 = 0.065;
+    public static double stack4 = 0.09;
+    public static double stack5 = 0.1;
+
+    public static double stack6 = 0.13;
+    public static double up = 0.30;
+    public static double motorPower = 0;
+
+    public Intake(HardwareMap hardwareMap, Position up) {
+        lServo = hardwareMap.get(Servo.class, "LeftIntake");
+        lServo.setDirection(Servo.Direction.REVERSE);
+        rServo = hardwareMap.get(Servo.class, "Right Intake Servo");
+        dcMotor = hardwareMap.get(DcMotor.class, "Intakemotor");
+        WheelServo = hardwareMap.get(CRServo.class, "Wheel");
+
+    }
+
+    public void setpos(Position stack) {
+        if (stack == Position.STACK1) {
+            lServo.setPosition(stack1);
+            rServo.setPosition(stack1);
+        } else if (stack == Position.STACK2) {
+            lServo.setPosition(stack2);
+            rServo.setPosition(stack2);
+        } else if (stack == Position.STACK3) {
+            lServo.setPosition(stack3);
+            rServo.setPosition(stack3);
+        } else if (stack == Position.STACK4) {
+            lServo.setPosition(stack4);
+            rServo.setPosition(stack4);
+        } else if (stack == Position.STACK5) {
+            lServo.setPosition(stack5);
+            rServo.setPosition(stack5);
+        } else if (stack == Position.STACK6) {
+            lServo.setPosition(stack6);
+            rServo.setPosition(stack6);
+        }
+        else if (stack == Position.UP) {
+            lServo.setPosition(up);
+            rServo.setPosition(up);
+        }
+    }
+
+    public void incrementPos() {
+        pos = pos.nextPosition();
+    }
+
+    public void decrementPos() {
+        pos = pos.previousPosition();
+    }
+
+    public void setDcMotor(double pwr) {
+        dcMotor.setPower(pwr);
+        if (pwr >= 0.01) {
+            this.setpos(this.pos);
+        }
+        else {
+            this.setpos(Position.UP);
+        }
+    }
+
+
+    public void wheel_spit(){
+
+        WheelServo.setPower(1);
+    }
+
+    public void wheel_swallow(){
+        WheelServo.setPower(-1);
+    }
+
+    public void wheel_stop(){
+        WheelServo.setPower(0);
+    }
+
+    public double getPower() {
+        return dcMotor.getPower();
+    }
+
+    public String getTelemetry() {
+        return "lServo: "+lServo.getPosition()+"rServo: "+rServo.getPosition()+"dcMotor: "+dcMotor.getPower();
+    }
+}

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

@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import java.util.Locale;
+
+@Config
+public class LEDs {
+    public static int NUMBER = 1;//80 solid
+    public static int RED = 80;
+    public static int REDFULL = 3;
+    public static int REDSCORING = 5;//5
+    public static int REDINIT = 79;
+    public static int BLUE = 93;
+    public static int BLUEFULL = 2;
+    public static int BLUESCORING = 5;//5
+    public static int BLUEINIT = 99;
+
+    public static int yellow = 81;
+    public static int lime = 87;
+    public static int white = 97;
+    public static int purple = 78;
+
+    //yellow:81, green: 87, white:97 , purple:78 ,
+    private RevBlinkinLedDriver blinkinLedDriver;
+    private RevBlinkinLedDriver.BlinkinPattern pattern;
+
+    public LEDs(HardwareMap hardwareMap) {
+        blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "LEDs");
+        setPattern();
+    }
+
+    public void setPattern() {
+        pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(NUMBER);
+        blinkinLedDriver.setPattern(pattern);
+    }
+
+    public void setPattern(int patternNumber) {
+        pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(patternNumber);
+        blinkinLedDriver.setPattern(pattern);
+    }
+
+    public void nextPattern() {
+        pattern = pattern.next();
+        blinkinLedDriver.setPattern(pattern);
+    }
+
+    public void previousPattern() {
+        pattern = pattern.previous();
+        blinkinLedDriver.setPattern(pattern);
+    }
+
+    public String getTelemetry() {
+        return String.format(Locale.US, "Pattern: %s", pattern.toString());
+    }
+}

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

@@ -0,0 +1,246 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Config
+public class Robot {
+
+    //Create each object
+    public SampleMecanumDrive drive;
+    public Camera camera;
+    public Intake intake;
+    public Slides slides;
+    public Arm arm;
+    public LEDs leds;
+    public endGame_Mechs endGameMechs;
+
+    public double macroStartTime = 0;
+    public int macroState = 0;
+    public int runningMacro = 0; // 0 = no macro | 1 = tier 1 | 2 = tier 2 | 3 = tier 3 | 4 = return
+    public int lastMacro = 0;
+
+    private boolean camEnabled = true;
+    public static double slideWait = 1.5;
+    public static double scoreWait = 0.65;
+    public static double armWait = 2.0;
+
+    public int alliance = 0;
+
+    //Name the objects
+    public Robot(HardwareMap hardwareMap) {
+        drive = new SampleMecanumDrive(hardwareMap);
+        camera = new Camera(hardwareMap);
+        intake = new Intake(hardwareMap, UP);
+        slides = new Slides(hardwareMap);
+        arm = new Arm(hardwareMap);
+        endGameMechs = new endGame_Mechs(hardwareMap);
+        camEnabled = true;
+        leds = new LEDs(hardwareMap);
+    }
+
+    public void extendMacro(int slidePos, double runTime) {
+        if (alliance == 1){
+            leds.setPattern(2);
+        }else{
+            leds.setPattern(3);
+        }
+
+        switch(macroState) {
+            case(0):
+                macroStartTime = runTime;
+                slides.setTarget(slidePos);
+                arm.setDoor(CLOSE);
+                macroState ++;
+                break;
+            case(1):
+                if (runTime > macroStartTime + slideWait || slides.atTarget()) {
+                    macroState ++;
+                }
+                break;
+            case(2):
+                arm.setArmPos(Arm.Position.SCORE);
+                arm.setWristPos(Arm.Position.SCORE);
+                macroState++;
+                break;
+            case (3):
+                if(arm.armAtPosition()){
+                    macroState = 0;
+                    lastMacro = runningMacro;
+                    runningMacro = 0;
+                    if (alliance == 1){
+                        leds.setPattern(93);
+                    }else{
+                        leds.setPattern(80);
+                    }
+                }
+                break;
+        }
+    }
+
+    public void resetMacro(int slidePos, double runTime) {
+        if (alliance == 1){
+            leds.setPattern(2);
+        }else{
+            leds.setPattern(3);
+        }
+
+        switch(macroState) {
+            case(0):
+                macroStartTime = runTime;
+                arm.setDoor(OPEN);
+                macroState++;
+                break;
+            case(1):
+                if (runTime > macroStartTime + scoreWait) {
+                        macroState ++;
+                }
+                break;
+            case(2):
+                macroStartTime = runTime;
+                arm.setArmPos(Arm.Position.INTAKE);
+                arm.setWristPos(Arm.Position.INTAKE);
+                macroState++;
+                break;
+            case(3):
+                if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) {
+                    macroState ++;
+                }
+                break;
+            case(4):
+                slides.setTarget(slidePos);
+                macroState++;
+                break;
+            case (5):
+                if (slides.atTarget()){
+                    macroState = 0;
+                    lastMacro = runningMacro;
+                    runningMacro = 0;
+                    if (alliance == 1){
+                        leds.setPattern(93);
+                    }else{
+                        leds.setPattern(80);
+                    }
+                }
+                break;
+        }
+    }
+    public void extendMacro_auto(int slidePos, double runTime, int slidepos2 ) {
+        switch(macroState) {
+            case(0):
+                macroStartTime = runTime;
+                slides.setTarget(slidePos);
+                arm.setDoor(CLOSE);
+                macroState ++;
+                break;
+            case(1):
+                if (runTime > macroStartTime + slideWait || slides.atTarget()) {
+                    macroState ++;
+                }
+                break;
+            case(2):
+                arm.setArmPos(Arm.Position.SCORE);
+                arm.setWristPos(Arm.Position.SCORE);
+                macroState++;
+                break;
+            case (3):
+                if(arm.armAtPosition()){
+                    macroState = 0;
+                    lastMacro = runningMacro;
+                    runningMacro = 0;
+                    macroState++;
+                }
+                break;
+
+            case (4):
+                slides.setTarget(slidepos2);
+                break;
+        }
+    }
+
+
+    public void resetMacro_auto(int slidePos2, double runTime, int slidePos) {
+        switch(macroState) {
+            case(0):
+                macroStartTime = runTime;
+                arm.setDoor(OPEN);
+                macroState++;
+                break;
+            case(1):
+                if (runTime > macroStartTime + scoreWait) {
+                    slides.setTarget(slidePos);
+                    macroState ++;
+                }
+                break;
+            case(2):
+                macroStartTime = runTime;
+                arm.setArmPos(Arm.Position.INTAKE);
+                arm.setWristPos(Arm.Position.INTAKE);
+                macroState++;
+                break;
+            case(3):
+                if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) {
+                    macroState ++;
+                }
+                break;
+            case(4):
+                slides.setTarget(slidePos2);
+                macroState++;
+                break;
+            case (5):
+                if (slides.atTarget()){
+                    macroState = 0;
+                    lastMacro = runningMacro;
+                    runningMacro = 0;
+                }
+                break;
+        }
+    }
+
+    public void update(double runTime) {
+//        drive.getPoseEstimate().getX();
+//         = estimatedPose.getX();
+  Pose2d estimatedPose = null;
+//        if (camera != null) {
+//            estimatedPose = this.camera.estimatePoseFromAprilTag();
+//        }
+        //drive.update(estimatedPose);
+
+//        Pose2d p1 = drive.getPoseEstimate();
+//
+//        Pose2d p2 = this.camera.estimatePoseFromAprilTag();
+//
+//        double D = Math.sqrt(Math.pow((p2.getX()-p1.getX()),2) + Math.pow((p2.getY()-p1.getY()),2));
+//
+//        if (D >= 6 || D <= 1){
+//            estimatedPose = null;
+//        }else{
+//            estimatedPose = this.camera.estimatePoseFromAprilTag();
+//
+//        }
+
+        drive.update(/*estimatedPose*/);
+        slides.update(runTime);
+        arm.update();
+        //leds.setPattern();
+    }
+
+
+
+    public String getTelemetry() {
+        return String.format("Arm:\n------------\n%s\nSlides:\n------------\n%s\nIMU:\n------------\n%s" , arm.getTelemetry(), slides.getTelemetry(), drive.getExternalHeadingVelocity());
+    }
+
+    public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
+        return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
+    }
+}

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

@@ -0,0 +1,127 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+@Config
+public class Slides {
+    public DcMotor slide;
+    public DcMotor slide2;
+
+//    public static double p = 0.0014;
+//    public static double i = 0.02;
+//    public static double d = 0;
+//    public static double f = 0.01;
+    //p was 0.0014
+    public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.04,0,0.01);
+    public static double pTolerance = 20;
+    private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d);
+
+    public static int targetMin = -10;
+    public static int targetMax = 830;
+
+    public static int down = 0;
+
+    public static int micro_tier1 = 50;
+    public static int mini_tier1 = 250;
+    public static int tier1 = 350;
+    public static int tier2 = 500;
+    public static int tier3 = 720;
+
+    private int target = 0;
+
+    public static int manualSpeed = 50;
+
+    public enum Position { DOWN, mini_tier1,  TIER1, TIER2, TIER3 }
+
+    public Slides(HardwareMap hardwareMap) {
+        slide = hardwareMap.get(DcMotor.class, "Rightslide");
+        slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        slide.setDirection(DcMotorSimple.Direction.REVERSE);
+        slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        slide2 = hardwareMap.get(DcMotor.class, "Leftslide");
+        slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+//        slide2.setDirection(DcMotorSimple.Direction.REVERSE);
+        slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    public void setTarget(int pos) {
+        target = Math.min(Math.max(pos, targetMin), targetMax);
+    }
+
+    public void setTarget(Position pos) {
+        if (pos == Position.DOWN) {
+            target = Math.min(Math.max(down, targetMin), targetMax);
+        } else if (pos == Position.mini_tier1) {
+            target = Math.min(Math.max(mini_tier1, targetMin), targetMax);
+        } else if (pos == Position.TIER1) {
+            target = Math.min(Math.max(tier1, targetMin), targetMax);
+        } else if (pos == Position.TIER2) {
+            target = Math.min(Math.max(tier2, targetMin), targetMax);
+        } else if (pos == Position.TIER3) {
+            target = Math.min(Math.max(tier3, targetMin), targetMax);
+        }
+    }
+
+    public void increaseTarget(double increase) {
+        target += (int) (increase * manualSpeed);
+        target = Math.min(targetMax, Math.max(targetMin, target));
+    }
+
+    public int getTarget() {
+        return target;
+    }
+
+
+
+    public boolean atTarget() {
+        return controller.atSetPoint();
+    }
+
+    public void cancel() {
+        target = slide.getCurrentPosition();
+    }
+
+    public void targetReset() {
+        target = targetMin;
+    }
+
+    public void update(double runTime) {
+//        highPos = 720 + heightOffset;
+//        midPos = 350 + heightOffset;
+//        lowPos = heightOffset;
+//        pickupPos = 20 + heightOffset;
+//        downPos = heightOffset;// TODO add these back in
+
+//        if (slide.getCurrentPosition() <= pTolerance && target <= pTolerance) {
+//            slide.setPower(0);
+//            slide2.setPower(0);
+//        } else {
+
+            double pid, ff;
+            controller.setPID(coefficients.p, coefficients.i, coefficients.d);
+            controller.setTolerance(pTolerance);
+
+            pid = controller.calculate(slide.getCurrentPosition(), target);
+            ff = coefficients.f;
+            slide.setPower(pid + ff);
+
+            pid = controller.calculate(slide2.getCurrentPosition(), target);
+            ff = coefficients.f;
+            slide2.setPower(pid + ff);
+//            }
+//        }
+        }
+
+
+    public String getTelemetry() {
+        return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());
+    }
+}

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

@@ -0,0 +1,81 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Config
+public class endGame_Mechs {
+    private Servo servo;
+    private Servo hang1;
+    private Servo hang2;
+    private DcMotor hang;
+    public static double initPos = 0.42;
+    public static double launchPos = 0.8;
+    public static double finger_out = 0.48;
+    public static double finger_in = 0.2;
+    public static int initHang = -1000;
+    public static double hold = 0.8;
+    public static double release = 0.5;
+    public static double hold2 = 0.8;
+    public static double release2 = 0.8;
+
+    public static int HangUp = -860;
+    public static int Hanged = -30;
+    private int target = 0;
+    public static int down = 0;
+
+    public static int manualSpeed = 50;
+
+
+
+    public endGame_Mechs(HardwareMap hardwareMap) {
+        this.servo = hardwareMap.get(Servo.class, "Drone");
+        this.servo.setPosition(initPos);
+//        this.servo = hardwareMap.get(Servo.class, "Finger");
+//        this.servo.setPosition(finger_out);
+        this.hang = hardwareMap.get(DcMotor.class, "Hang");
+        this.hang.setTargetPosition(0);
+        this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        this.hang.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        this.hang.setDirection(DcMotorSimple.Direction.REVERSE);
+        this.hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        //        this.hang1 = hardwareMap.get(Servo.class, "Hanger 1");
+//        this.hang1.setPosition(hold);
+//        this.hang2 = hardwareMap.get(Servo.class, "Hanger 2");
+//        this.hang2.setPosition(hold);
+
+    }
+
+    public void launch() {
+        this.servo.setPosition(launchPos);
+    }
+
+    public void reset() {
+        this.servo.setPosition(initPos);
+    }
+
+
+//    public void Finger_in() {this.servo.setPosition(finger_in);}
+//
+//    public void Finger_out () {this.servo.setPosition(finger_out);}
+
+
+    public void hang_init_pos(){
+        this.hang.setTargetPosition(0);
+        this.hang.setPower(1);
+    }
+
+
+    public void hang_final_pos(){
+        this.hang.setTargetPosition(2250);
+        this.hang.setPower(1);
+    }
+
+
+
+}
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PoseStorage.java

@@ -0,0 +1,9 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+public class PoseStorage {
+    public static Pose2d currentPose = new Pose2d();
+    public static boolean AutoJustEnded = false;
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java

@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+import org.firstinspires.ftc.teamcode.vision.Detection;
+
+@Config
+public abstract class AutoBase extends LinearOpMode {
+    public Robot robot;
+
+    protected int macroState;
+    protected double macroTime;
+    protected int teamPropLocation = 2; // 1 is left, 2 is middle, 3 is right, perhaps these could be enums instead
+
+    // abstract methods for each auto to implement
+    public abstract void createTrajectories();
+    public abstract void followTrajectories();
+
+    public static int DetectionTest = 20;
+    @Override
+    public void runOpMode() {
+        // create telemetry object for both driver hub and dashboard
+        // check SampleMecanumDrive to make sure it doesn't override the dashboard telemetry
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        // initialize robot
+        robot = new Robot(hardwareMap);
+        //robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
+
+        // create trajectories
+        createTrajectories();
+
+        // wait for start
+        while (!isStarted() && !isStopRequested()) {
+//            if (!robot.camera.isCameraReady()) {
+//                telemetry.addLine("Initializing...");
+//            } else {
+                telemetry.addLine("Initialized");
+                // Detection vndafds = robot.camera.getProp() <- returns a detection
+                // int fdsf = detection.getCenter().x <- x value on the screen between -50,50
+                  double PropDetection = robot.camera.getProp().getCenter().x;
+
+                if (PropDetection <= -DetectionTest ) {
+                    teamPropLocation = 1;
+                } else if (PropDetection >= -DetectionTest && PropDetection <= DetectionTest) {
+                    teamPropLocation = 2;
+                } else if (PropDetection >= DetectionTest) {
+                    teamPropLocation = 3;
+                }
+
+                // teamPropLocation = 1,2,3
+                //teamPropLocation = robot.camera.getMarkerId(); // or whatever method you end up using
+                telemetry.addData("Team Prop Location", teamPropLocation);
+//            }
+            telemetry.update();
+        }
+
+        // run auto
+        while (macroState != -1 && !isStopRequested()) {
+            // state machine to follow trajectories
+            followTrajectories();
+
+            // update robot
+            robot.update(getRuntime());
+
+            // update telemetry
+            telemetry.addData("Macro State", macroState);
+            telemetry.addLine(robot.getTelemetry());
+            telemetry.update();
+        }
+
+        // stop camera
+        //robot.camera.stopTargetingCamera();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java

@@ -0,0 +1,309 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.hardware.Arm;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Blue Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp")
+public class BlueBackStageAuto extends AutoBase {
+    public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180));
+
+    public static final Pose2d DROP_3M = new Pose2d(13.8, 32, Math.toRadians(-180));
+    public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90));
+
+    public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
+
+    public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90));
+    public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53.4, 28.7, Math.toRadians(-180));
+    public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.5, 34.5, Math.toRadians(-180));
+    public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-180));
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51, 32.6, Math.toRadians(-180));
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-180));
+
+
+    //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36
+
+    public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180));
+
+    public static final Pose2d STACK_LOCATION2 = new Pose2d(-58, 11.1, Math.toRadians(-180));
+
+    public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180));
+
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3);
+                        builder.lineToLinearHeading(DROP_3M);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+//                if (getRuntime() < macroTime + 0.05) {
+//
+//                }
+//                // if intake is done move on
+//               else {
+                robot.intake.setDcMotor(0);
+                robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+                robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1);
+                builder = this.robot.getTrajectorySequenceBuilder();
+                //Scores yellow pixel
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DEPOSIT_PRELOAD_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                // }
+                break;
+            // EXTEND AND MOVE TO BACKBOARD
+            case 3:
+                // extend macro
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                // if macro and drive are done, move on
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            // STACK RUN 1 -------------------------
+            case 4:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
+                            break;
+                        case 2:
+                            builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0)));
+                            break;
+                        case 3:
+                            builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+            //waits for the robot to fin  the trajectory
+            case 5:
+                robot.resetMacro(0, getRuntime());
+                robot.intake.setDcMotor(0.58);
+                robot.intake.setpos(STACK6);
+                if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) {
+                    //robot.intake.setDcMotor(0.5);
+                    macroState++;
+                }
+                break;
+            //First 2 pixels off the stack are intaken by this
+            case 6:
+                robot.intake.setDcMotor(0.48);
+                robot.intake.setpos(STACK5);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+            //goes back to the easel
+            case 7:
+                if (getRuntime() > macroTime + 0.7) {
+                    //robot.intake.setDcMotor(-0.0);
+                    robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+                    robot.intake.setDcMotor(-0.35);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 8:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1 + 60, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 9:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1 + 60, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            // STACK RUN 2
+            case 10:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
+                            break;
+                        case 2:
+                            builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0)));
+                            break;
+                        case 3:
+                            builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+            //waits for the robot to fin  the trajectory
+            case 11:
+                robot.resetMacro(0, getRuntime());
+                robot.intake.setDcMotor(0.58);
+                robot.intake.setpos(STACK3);
+                if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) {
+                    //robot.intake.setDcMotor(0.68);
+                    macroState++;
+                }
+                break;
+            //Third and 4th pixels off the stack are intaken by this
+            case 12:
+                robot.intake.setDcMotor(0.48);
+                robot.intake.setpos(STACK2);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+            //goes back to the easel
+            case 13:
+                if (getRuntime() > macroTime + 0.7) {
+                    robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+                    robot.intake.setDcMotor(-0.35);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 14:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 15:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            // PARK ROBOT
+            case 16:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+
+                // PARK ROBOT
+//
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStagePark.java

@@ -0,0 +1,313 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.hardware.Arm;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Blue Backstage Auto(2 and Park)", group = "Competition", preselectTeleOp = "Main TeleOp")
+public class BlueBackStagePark extends AutoBase {
+    public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180));
+
+    public static final Pose2d DROP_3M = new Pose2d(13.8, 32, Math.toRadians(-180));
+    public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90));
+
+    public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
+
+    public static final Pose2d DROP_1 = new Pose2d(24.5, 43, Math.toRadians(-90));
+    public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180));
+    public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180));
+    public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187));
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187));
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187));
+
+
+    //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36
+
+    public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180));
+
+    public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180));
+
+    public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180));
+
+    public static final Pose2d PARK_1 = new Pose2d(-53,58,Math.toRadians(-180));
+
+    public static final Pose2d PARK_2 = new Pose2d(-58,58,Math.toRadians(-180));
+
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder = null;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3);
+                        builder.lineToLinearHeading(DROP_3M);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.1) {
+                    robot.intake.setDcMotor(-0.24);
+                }
+                // if intake is done move on
+                else {
+                    robot.intake.setDcMotor(0);
+                    robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //Scores yellow pixel
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
+                            break;
+                        case 2:
+                            builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
+                            break;
+                        case 3:
+                            builder.lineToLinearHeading(DEPOSIT_PRELOAD_3);
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+            // EXTEND AND MOVE TO BACKBOARD
+            case 3:
+                // extend macro
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                // if macro and drive are done, move on
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            // STACK RUN 1 -------------------------
+            case 4:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
+//                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+//                    builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.lineToLinearHeading(PARK_1);
+                    builder.lineToLinearHeading(PARK_2);
+
+//                    switch (teamPropLocation) {
+//                        case 1:
+//                            builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
+//                            break;
+//                        case 2:
+//                            builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0)));
+//                            break;
+//                        case 3:
+//                            builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5)));
+//                            break;
+//                    }
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+                }
+                break;
+//            //waits for the robot to fin  the trajectory
+//            case 5:
+//                robot.resetMacro(0, getRuntime());
+//                robot.intake.setDcMotor(0.68);
+//                robot.intake.setpos(STACK5);
+//                if (!robot.drive.isBusy()) {
+//                    macroState++;
+//                }
+//                break;
+//            //First 2 pixels off the stack are intaken by this
+//            case 6:
+//                robot.intake.setDcMotor(0.68);
+//                robot.intake.setpos(STACK5);
+//                macroTime = getRuntime();
+//                macroState++;
+//                break;
+//            //goes back to the easel
+//            case 7:
+//                if (getRuntime() > macroTime + 0.03) {
+//                    //robot.intake.setDcMotor(-0.0);
+//                    robot.intake.setDcMotor(-0.35);
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+//                    builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
+//                    switch (teamPropLocation) {
+//                        case 1:
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+//                            break;
+//                        case 2:
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+//                            break;
+//                        case 3:
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+//                            break;
+//                    }
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+//                    macroTime = getRuntime();
+//                }
+//                break;
+//            case 8:
+//                // if drive is done move on
+//                if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
+//                    macroTime = getRuntime();
+//                    robot.macroState = 0;
+//                    robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
+//                    macroState++;
+//                }
+//                break;
+//            //extending the macro and about to score
+//            case 9:
+//                if (robot.macroState != 0) {
+//                    robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
+//                }
+//                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+//                    robot.resetMacro(0, getRuntime());
+//                    macroState++;
+//                    macroTime = getRuntime();
+//                }
+//                break;
+//            // STACK RUN 2
+//            case 10:
+//                robot.resetMacro(0, getRuntime());
+//                if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+//                    builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
+//                    switch (teamPropLocation) {
+//                        case 1:
+//                            builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
+//                            break;
+//                        case 2:
+//                            builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2)));
+//                            break;
+//                        case 3:
+//                            builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5
+//                            )));
+//                            break;
+//                    }
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+//                }
+//                break;
+//            //waits for the robot to fin  the trajectory
+//            case 11:
+//                robot.resetMacro(0, getRuntime());
+//                robot.intake.setDcMotor(0.68);
+//                robot.intake.setpos(STACK3);
+//                if (!robot.drive.isBusy()) {
+//                    macroState++;
+//                }
+//                break;
+//            //Third and 4th pixels off the stack are intaken by this
+//            case 12:
+//                robot.intake.setDcMotor(0.68);
+//                robot.intake.setpos(STACK2);
+//                macroTime = getRuntime();
+//                macroState++;
+//                break;
+//            //goes back to the easel
+//            case 13:
+//                if (getRuntime() > macroTime + 0.03) {
+//                    robot.intake.setDcMotor(-0.35);
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+//                    builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
+//                    switch (teamPropLocation) {
+//                        case 1:
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+//                            break;
+//                        case 2:
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+//                            break;
+//                        case 3:
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+//                            break;
+//                    }
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+//                    macroTime = getRuntime();
+//                }
+//                break;
+//            case 14:
+//                // if drive is done move on
+//                if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
+//                    macroTime = getRuntime();
+//                    robot.macroState = 0;
+//                    robot.extendMacro(Slides.mini_tier1 + 140, getRuntime());
+//                    macroState++;
+//                }
+//                break;
+//            //extending the macro and about to score
+//            case 15:
+//                if (robot.macroState != 0) {
+//                    robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
+//                }
+//                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+//                    robot.resetMacro(0, getRuntime());
+//                    macroState++;
+//                }
+//                break;
+//
+//            // PARK ROBOT
+            case 5:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+//                    builder.lineToLinearHeading(PARK_1);
+//                    builder.lineToLinearHeading(PARK_2);
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+                }
+//
+//                // PARK ROBOT
+////
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java

@@ -0,0 +1,236 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
+public class BlueFrontPreload extends AutoBase {
+    public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0));
+
+    public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0));
+    public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
+    public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
+    public static final Pose2d DROP_3 = new Pose2d(-51, 50.5, Math.toRadians(-90));
+    public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90));
+
+    public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
+
+    public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90));
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817
+
+    public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180));
+
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
+
+    public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180));
+
+    public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180));
+
+    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180));
+
+    public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
+
+    public static final Pose2d PARK_2 = new Pose2d(58,58,Math.toRadians(-180));
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder = null;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1M);
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3M);
+//                        builder.lineToLinearHeading(DROP_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.18) {
+                    robot.intake.setDcMotor(-0.23);
+                }
+                else{
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToLinearHeading(DROP_1_PART_2);
+                            break;
+                        case 2:
+                            builder.lineToLinearHeading(DROP_2_PART_2);
+                            break;
+                        case 3:
+                            builder.lineToLinearHeading(DROP_3);
+                            break;
+                    }
+                    robot.arm.setDoor(OPEN);
+                    switch (teamPropLocation) {
+                        case (1):
+                            //team prop location 1
+                            builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
+                            break;
+                        case (2):
+                            //team prop location 2
+                            builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
+                            break;
+                        case (3):
+                            //team prop location 3
+                            builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
+                            break;
+                    }
+
+
+                    robot.intake.setDcMotor(0.44);
+                    robot.intake.setpos(STACK6);
+                    macroTime = getRuntime();
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                // if intake is done move on
+                break;
+            case 3:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+
+
+            case 4:
+                if (getRuntime() > macroTime + 0.06) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.5);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 5:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 6:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            //Park
+
+            case 7:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.lineToLinearHeading(PARK_1);
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+            case 8:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+                    macroState++;
+                }
+                break;
+
+            case 9:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+
+            case 19:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java

@@ -0,0 +1,292 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Blue FrontPreload (2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
+public class BlueFrontStageAuto extends AutoBase {
+    public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0));
+
+    public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0));
+    public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
+    public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
+    public static final Pose2d DROP_3 = new Pose2d(-49, 50.5, Math.toRadians(-90));
+    public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90));
+
+    public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
+
+    public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90));
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817
+
+    public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180));
+
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
+
+    public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180));
+
+    public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180));
+
+    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180));
+
+    public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
+
+    public static final Pose2d PARK_2 = new Pose2d(58,58,Math.toRadians(-180));
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder = null;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1M);
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3M);
+//                        builder.lineToLinearHeading(DROP_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.18) {
+                    robot.intake.setDcMotor(-0.23);
+                }
+                else{
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToLinearHeading(DROP_1_PART_2);
+                            break;
+                        case 2:
+                            builder.lineToLinearHeading(DROP_2_PART_2);
+                            break;
+                        case 3:
+                            builder.lineToLinearHeading(DROP_3);
+                            break;
+                    }
+                    robot.arm.setDoor(OPEN);
+                    switch (teamPropLocation) {
+                        case (1):
+                            //team prop location 1
+                            builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
+                            break;
+                        case (2):
+                            //team prop location 2
+                            builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
+
+                        case (3):
+                            //team prop location 3
+                            builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
+
+                    }
+
+
+                    robot.intake.setDcMotor(0.44);
+                    robot.intake.setpos(STACK6);
+                    macroTime = getRuntime();
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                // if intake is done move on
+                break;
+            case 3:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+
+
+            case 4:
+                if (getRuntime() > macroTime + 0.06) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.5);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 5:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 6:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            case 7:
+                robot.resetMacro(0,getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
+                    builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
+                    builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+            case 8:
+                robot.arm.setDoor(OPEN);
+                robot.intake.setDcMotor(0.44);
+                robot.intake.setpos(STACK4);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+
+            case 9:
+                if (getRuntime() > macroTime + 0.06) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.5);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+
+            case 10:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            case 11:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            //Park
+
+            case 12:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.lineToLinearHeading(PARK_1);
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+
+
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java

@@ -0,0 +1,320 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.hardware.Arm;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Red Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp")
+public class RedBackStageAuto extends AutoBase {
+
+    public static final Pose2d DROP_1 = new Pose2d(16, -32.5, Math.toRadians(180));
+
+    public static final Pose2d DROP_1M = new Pose2d(11.5, -32.5, Math.toRadians(180));
+
+    public static final Pose2d DROP_2 = new Pose2d(15.6, -33.5, Math.toRadians(90));
+
+    public static final Pose2d ALINE = new Pose2d(51,-32.5, Math.toRadians(180));
+
+    public static final Pose2d DROP_3 = new Pose2d(25, -42.5, Math.toRadians(90));
+    public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -26.3, Math.toRadians(180));
+    public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, -32.5, Math.toRadians(180));
+    public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.3, -39, Math.toRadians(180));
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(52, -35.3, Math.toRadians(180));//187
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -30.5, Math.toRadians(180));//187
+
+    public static  final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(52.4, -34.5, Math.toRadians(180));//187
+
+
+    //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11, Math.toRadians(180));//-36
+
+    public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11, Math.toRadians(185));
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1);
+                        builder.lineToLinearHeading(DROP_1M);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.26) {
+                    robot.intake.setDcMotor(-0.18);
+                }
+                // if intake is done move on
+                else {
+                    robot.intake.setDcMotor(0);
+                    robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+                    robot.extendMacro_auto(0, getRuntime(),Slides.mini_tier1);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //Scores yellow pixel
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
+                            break;
+                        case 2:
+                            //robot.extendMacro(Slides.mini_tier1 -40, getRuntime());
+                            builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
+                            break;
+                        case 3:
+                            builder.lineToLinearHeading(DEPOSIT_PRELOAD_3);
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+            // EXTEND AND MOVE TO BACKBOARD
+            case 3:
+                // extend macro
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1 - 20, getRuntime());
+                }
+                // if macro and drive are done, move on
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            // STACK RUN 1 -------------------------
+            case 4:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2)));
+                            robot.intake.setpos(STACK5);
+                            break;
+                        case 2:
+                            builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-2)));
+                            robot.intake.setpos(STACK5);
+                            break;
+                        case 3:
+                            builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2)));
+                            robot.intake.setpos(STACK5);
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            //waits for the robot to fin  the trajectory
+            case 5:
+
+                    robot.resetMacro(0, getRuntime());
+                    robot.intake.setDcMotor(0.58);
+                    robot.intake.setpos(STACK6);
+
+                if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
+                    macroState++;
+                }
+                break;
+            //First 2 pixels off the stack are intaken by this
+            case 6:
+                robot.intake.setDcMotor(0.48);
+                robot.intake.setpos(STACK5);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+            //goes back to the easel
+            case 7:
+                if (getRuntime() > macroTime + 0.6) {
+                    robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
+                    robot.intake.setDcMotor(-0.35);
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 8:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1 , getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 9:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1 , getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            // STACK RUN 2
+            case 10:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) {
+                    robot.intake.setDcMotor(0);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.75)));
+                            robot.intake.setpos(STACK3);
+                            break;
+                        case 2:
+                            builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3.3)));
+                            robot.intake.setpos(STACK3);
+                            break;
+                        case 3:
+                            builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8)));
+                            robot.intake.setpos(STACK3);
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            //waits for the robot to fin  the trajectory
+            case 11:
+
+                    robot.resetMacro(0, getRuntime());
+                    robot.intake.setDcMotor(0.58);
+                    robot.intake.setpos(STACK3);
+
+                if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
+                    macroState++;
+                }
+                break;
+            //3rd and 4th pixels off the stack are intaken by this
+            case 12:
+                robot.intake.setDcMotor(0.48);
+                robot.intake.setpos(STACK2);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+            //goes back to the easel
+            case 13:
+                if (getRuntime() > macroTime + 0.7) {
+                    robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
+                    robot.intake.setDcMotor(-0.35);
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 14:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1 + 100, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 15:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1 + 100, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            // PARK ROBOT
+            case 16:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+
+                // PARK ROBOT
+//
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java

@@ -0,0 +1,235 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Red Teleop")
+public class RedFrontPreload extends AutoBase {
+    public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
+
+    public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
+    public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
+    public static final Pose2d DROP_2_PART_2 = new Pose2d(-37.7,-42, Math.toRadians(90));
+    public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
+    public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
+
+    public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
+
+    public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(47, -32, Math.toRadians(187));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
+
+    public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
+
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
+
+    public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
+
+    public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(0));
+
+    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(0));
+
+    public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
+
+    public static final Pose2d PARK_2 = new Pose2d(58,-58,Math.toRadians(180));
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder = null;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1M);
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3M);
+//                        builder.lineToLinearHeading(DROP_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.1) {
+                }
+                else{
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToLinearHeading(DROP_1_PART_2);
+                            break;
+                        case 2:
+                            builder.lineToLinearHeading(DROP_2_PART_2);
+                            break;
+                        case 3:
+                            builder.lineToLinearHeading(DROP_3);
+                            break;
+                    }
+                    robot.arm.setDoor(OPEN);
+                    switch (teamPropLocation) {
+                        case (1):
+                            //team prop location 1
+                            builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
+                            break;
+                        case (2):
+                            //team prop location 2
+                            builder.lineToLinearHeading(STACK_LOCATION_2).waitSeconds(.01);
+                            break;
+                        case (3):
+                            //team prop location 3
+                            builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,-2))).waitSeconds(.01);
+                            break;
+                    }
+
+
+                    robot.intake.setDcMotor(0.44);
+                    robot.intake.setpos(STACK6);
+                    macroTime = getRuntime();
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                // if intake is done move on
+                break;
+            case 3:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+
+            case 4:
+                if (getRuntime() > macroTime + 0.1) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.1);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec().plus(new Vector2d((3.8))),Math.toRadians(180));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 5:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 6:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            //Park
+
+            case 7:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.lineToLinearHeading(PARK_1);
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+            case 8:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+                    macroState++;
+                }
+                break;
+
+            case 9:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+
+            case 19:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java

@@ -0,0 +1,365 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "JANK Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
+public class RedFrontStageAuto extends AutoBase {
+    public static final Pose2d DROP_1 = new Pose2d(-35, -32.5, Math.toRadians(180)); //THIS ANGLE NEEDS TO BE CHANGED
+    public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90));
+    public static final Pose2d DROP_3 = new Pose2d(-49, -33.5, Math.toRadians(90));
+    public static final Pose2d DROP_1M = new Pose2d(-48.5, -30, Math.toRadians(90));
+
+    public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
+
+    public static final Pose2d DROP_3M = new Pose2d(-48.5, -30, Math.toRadians(90));
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -32, Math.toRadians(188));//817
+
+    public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, -33.6, Math.toRadians(0));
+
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
+
+    public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
+
+    public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0));
+
+    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0));
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder = null;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.32) {
+                    robot.intake.setDcMotor(-0.18);
+                }
+                else{
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    robot.arm.setDoor(OPEN);
+                    builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,1.5))).waitSeconds(.01);
+
+
+                    robot.intake.setDcMotor(0.44);
+                    robot.intake.setpos(STACK6);
+                    macroTime = getRuntime();
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                // if intake is done move on
+                break;
+            case 3:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+
+
+            case 4:
+                if (getRuntime() > macroTime + 0.06) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.8);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+//                    builder.lineToConstantHeading(POST_DROP_POS.vec());
+//                    builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0));
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180));
+                            break;
+//                        case 1:
+//                            builder.setTangent(Math.toRadians(90))
+//                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+//                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+//                            break;
+//                        case 2:
+//                            builder.lineToLinearHeading(POST_DROP_POS);
+//                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+//                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
+//                            break;
+//                        case 3:
+//                            builder.lineToLinearHeading(POST_DROP_POS);
+//                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+//                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
+//                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 5:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-70, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 6:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            //Stack run 2
+            case 7:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.splineToConstantHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,2)).vec(), Math.toRadians(0));
+                    builder.lineToConstantHeading(POST_DROP_POS.plus(new Pose2d(0,2)).vec());
+                    builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.9, 3.7)).vec(), Math.toRadians(0));
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+            //waits for the robot to fin  the trajectory
+            case 8:
+                robot.resetMacro(0, getRuntime());
+                robot.arm.setDoor(OPEN);
+                robot.intake.setDcMotor(0.54);
+                robot.intake.setpos(STACK4);
+                if (!robot.drive.isBusy()) {
+                    macroState++;
+                }
+                break;
+            //3rd and 4th pixels off the stack are in-taken by this
+            case 9:
+                robot.intake.setDcMotor(0.54);
+                robot.arm.setDoor(OPEN);
+                robot.intake.setpos(STACK3);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+
+            case 10:
+                if (getRuntime() > macroTime + 0.6) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.45);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+                    //builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0));
+                    //builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2)));
+
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 11:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1 + 20, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 12:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+            case 13:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+
+                //stack run 3
+//            case 13:
+//                robot.resetMacro(0, getRuntime());
+//                if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) {
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(180));
+//                    builder.lineToLinearHeading(POST_DROP_POS);
+//                    builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180));
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+//                }
+//                break;
+//
+//                //waits for the robot to fin  the trajectory
+//            case 14:
+//                robot.resetMacro(0, getRuntime());
+//                robot.arm.setDoor(OPEN);
+//                robot.intake.setDcMotor(0.54);
+//                robot.intake.setpos(STACK2);
+//                if (!robot.drive.isBusy()) {
+//                    macroState++;
+//                }
+//                break;
+//            //3rd and 4th pixels off the stack are intaken by this
+//            case 15:
+//                robot.intake.setDcMotor(0.54);
+//                robot.arm.setDoor(OPEN);
+//                robot.intake.setpos(STACK1);
+//                macroTime = getRuntime();
+//                macroState++;
+//                break;
+//
+//            case 16:
+//                if (getRuntime() > macroTime + 0.6) {
+//                    robot.intake.setDcMotor(0);
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
+//                    builder.splineToSplineHeading(POST_DROP_POS, Math.toRadians(0));
+//
+//                    switch (teamPropLocation) {
+//                        case 1:
+//                            builder.lineToLinearHeading(PRE_DEPOSIT_POS);
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0));
+//                            break;
+//                        case 2:
+//                            builder.lineToLinearHeading(PRE_DEPOSIT_POS);
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
+//                            break;
+//                        case 3:
+//                            builder.lineToLinearHeading(PRE_DEPOSIT_POS);
+//                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
+//                            break;
+//                    }
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+//                    macroTime = getRuntime();
+//                }
+//                break;
+//            case 17:
+//                // if drive is done move on
+//                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+//                    macroTime = getRuntime();
+//                    robot.macroState = 0;
+//                    robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
+//                    macroState++;
+//                }
+//                break;
+//            //extending the macro and about to score
+//            case 18:
+//                if (robot.macroState != 0) {
+//                    robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
+//                }
+//                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+//                    robot.resetMacro(0, getRuntime());
+//                    macroState++;
+//                }
+//                break;
+            case 19:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+        }
+    }
+}
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java

@@ -0,0 +1,304 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Red FrontPreload (2+2)", group = "Competition", preselectTeleOp = "Red Teleop")
+public class RedFrontStageme extends AutoBase {
+    public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
+
+    public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
+    public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
+
+    public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90));
+    public static final Pose2d DROP_2_PART_3 = new Pose2d(-40.7,-42, Math.toRadians(180));
+    public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
+    public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
+
+    public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
+
+    public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -32, Math.toRadians(187));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
+
+    public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
+
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
+
+    public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
+
+    public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(180));
+
+    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(180));
+
+    public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
+
+    public static final Pose2d PARK_2 = new Pose2d(58,-58,Math.toRadians(180));
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder = null;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1M);
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3M);
+//                        builder.lineToLinearHeading(DROP_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.1) {
+                }
+                else{
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToLinearHeading(DROP_1_PART_2);
+                            break;
+                        case 2:
+                            builder.lineToLinearHeading(DROP_2_PART_2);
+                            builder.lineToLinearHeading(DROP_2_PART_3);
+                            break;
+                        case 3:
+                            builder.lineToLinearHeading(DROP_3);
+                            break;
+                    }
+                    //robot.arm.setDoor(OPEN);
+                    macroTime = getRuntime();
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                // if intake is done move on
+                break;
+            case 3:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+
+            case 4:
+                if (getRuntime() > macroTime + 0.1) {
+                    //robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.1);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 5:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 6:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            case 7:
+                robot.resetMacro(0,getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
+                    builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
+                    builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+            case 8:
+                robot.arm.setDoor(OPEN);
+                robot.intake.setDcMotor(0.44);
+                robot.intake.setpos(STACK5);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+
+            case 9:
+                if (getRuntime() > macroTime + 0.06) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.5);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+
+            case 10:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            case 11:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            //Park
+
+            case 12:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.lineToLinearHeading(PARK_1);
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+            //Park
+
+//            case 7:
+//                robot.resetMacro(0, getRuntime());
+//                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    builder.lineToLinearHeading(PARK_1);
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+//                }
+//                break;
+//
+//            case 8:
+//                // if drive is done move on
+//                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+//                    macroState++;
+//                }
+//                break;
+//
+//            case 9:
+//                robot.resetMacro(0, getRuntime());
+//                if (robot.macroState == 0) {
+//                    macroState = -1;
+//                }
+
+            case 19:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java

@@ -0,0 +1,259 @@
+package org.firstinspires.ftc.teamcode.opmode.teleop;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.controller.Controller;
+import org.firstinspires.ftc.teamcode.hardware.Arm;
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+
+@Config
+@TeleOp(name = "Blue TeleOp", group = "OpModes")
+public class BlueTeleop extends OpMode {
+
+    public static double normal = 0.7;
+    public static double turbo = 1;
+    public static double slow_mode = 0.35;
+    public static double intakeMax = 0.36;
+    public static double intakeMax2 = -0.70;
+
+    private Robot robot;
+    private Controller controller1;
+    private Controller controller2;
+
+    public boolean hang_counter = false;
+
+
+    private DcMotor[] motors = new DcMotor[8];
+    private Servo[] servos = new Servo[7];
+
+    @Override
+    public void init() {
+        this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
+        this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
+        this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
+        this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
+        this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
+        this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
+        this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
+        this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
+
+        this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
+        this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
+        this.servos[2] = this.hardwareMap.get(Servo.class, "Elbow");
+        this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
+        this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
+        this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
+        this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
+
+
+        controller1 = new Controller(gamepad1);
+        controller2 = new Controller(gamepad2);
+
+        this.robot = new Robot(hardwareMap);
+//        robot.intake.setpos(Intake.Position.STACK1);
+
+//        while (robot.camera.getFrameCount() < 1) {
+//            telemetry.addLine("Initializing...");
+//            telemetry.update();
+//        }
+
+        telemetry.addLine("Initialized");
+        this.telemetry = FtcDashboard.getInstance().getTelemetry();
+        telemetry.update();
+
+        this.robot.alliance = 1;
+        this.robot.leds.setPattern(92);
+
+    }
+
+    double getBatteryVoltage() {
+        double result = Double.POSITIVE_INFINITY;
+        for (VoltageSensor sensor : hardwareMap.voltageSensor) {
+            double voltage = sensor.getVoltage();
+            if (voltage > 0) {
+                result = Math.min(result, voltage);
+            }
+        }
+        return result;
+    }
+
+    @Override
+    public void loop() {
+        for (DcMotor motor : this.motors) {
+            this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
+        }
+        for (Servo servo : this.servos) {
+            this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
+        }
+
+        this.telemetry.addData("battery", getBatteryVoltage());
+        this.telemetry.update();
+
+        this.robot.leds.setPattern(93);
+
+        // Driver 1
+        double x = -gamepad1.left_stick_y;
+        double y = -gamepad1.left_stick_x;
+        double z = -gamepad1.right_stick_x;
+
+        if (controller1.getRightTrigger().getValue() > 0.1) {
+            x *= turbo;
+            y *= turbo;
+            z *= turbo;
+        }
+        else if (controller1.getLeftTrigger().getValue() > 0.1) {
+            x *= slow_mode;
+            y *= slow_mode;
+            z *= slow_mode;
+        } else {
+            x *= normal;
+            y *= normal;
+            z *= normal;
+        }
+        robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
+        // Drone launcher
+        if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+            this.robot.endGameMechs.launch();
+        } else {
+            this.robot.endGameMechs.reset();
+        }
+
+//        if (controller1.getRightBumper().isPressed()) {
+//            this.robot.endGameMechs.Finger_in();
+//        }else {
+//            this.robot.endGameMechs.Finger_out();
+//        }
+        //Hang Motor
+
+//        if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
+//            this.robot.endGameMechs.hang_init_pos();
+//            hang_counter = true;
+//        }
+//        else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){
+//            this.robot.endGameMechs.hang_final_pos();
+//            hang_counter = false;
+//        }
+
+
+        if (controller1.getB().isPressed()) {
+            this.robot.endGameMechs.hang_final_pos();
+        } else  {
+            this.robot.endGameMechs.hang_init_pos();
+        }
+
+        if (controller1.getX().isPressed()) {
+            this.robot.intake.wheel_spit();
+        } else if (controller1.getY().isPressed()) {
+            this.robot.intake.wheel_swallow();
+        } else {
+            this.robot.intake.wheel_stop();
+        }
+
+
+        // Driver 2
+        if (controller2.getRightTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
+        }
+        else if(controller2.getLeftTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
+        }
+        else {
+            robot.intake.setDcMotor(0);
+        }
+
+        if (controller2.getRightBumper().isJustPressed()) {
+            robot.intake.incrementPos();
+        }
+        if (controller2.getLeftBumper().isJustPressed()) {
+            robot.intake.decrementPos();
+        }
+
+
+        //ElbowPos
+        if (controller2.getDRight().isPressed()){
+            robot.arm.setElbowPos(4);
+        } else if (controller2.getDLeft().isPressed()) {
+            robot.arm.setElbowPos(3);
+        }else {
+            robot.arm.setElbowPos(1);
+        }
+
+
+        // macros
+        switch (robot.runningMacro) {
+            case (0): // manual mode
+
+                robot.slides.increaseTarget(controller2.getLeftStick().getY());
+
+//                if (robot.intake.getPower() >= 0.01) {
+//                    robot.arm.setDoor(OPEN);
+//                } else if (robot.intake.getPower() <= -0.01) {
+//                    robot.arm.setDoor(OPEN);
+//                } else if (controller2.getLeftBumper().isPressed()) {
+//                    robot.arm.setDoor(Arm.DoorPosition.OPEN);
+//                } else {
+//                    robot.arm.setDoor(CLOSE);
+//                }
+
+                if (robot.intake.getPower() >= 0.01) {
+                    robot.intake.wheel_swallow();
+                } else if (robot.intake.getPower() <= -0.01) {
+                    robot.intake.wheel_swallow();
+                } else if (controller2.getLeftBumper().isPressed()) {
+                    robot.intake.wheel_swallow();
+                } else {
+                    robot.intake.wheel_stop();
+                }
+
+                //Start A/B stuff
+                if (controller2.getX().isJustPressed()) {
+                    robot.runningMacro = 1;
+                } else if (controller2.getY().isJustPressed()) {
+                    robot.runningMacro = 2;
+                } else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 3;
+                } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 4;
+                } else if (controller2.getDDown().isJustPressed() ) {
+                    robot.runningMacro = 5;
+                }
+
+                break;
+            case (1):
+                robot.extendMacro(Slides.tier1, getRuntime());
+                break;
+            case (2):
+                robot.extendMacro(Slides.tier2, getRuntime());
+                break;
+            case (3):
+                robot.extendMacro(Slides.tier3, getRuntime());
+                break;
+            case (4):
+                robot.resetMacro(0, getRuntime());
+                break;
+            case(5):
+                robot.extendMacro(Slides.mini_tier1, getRuntime());
+                break;
+        }
+
+        // update and telemetry
+        robot.update(getRuntime());
+        controller1.update();
+        controller2.update();
+        telemetry.addLine(robot.getTelemetry());
+        telemetry.update();
+
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Drivebase.java

@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.opmode.teleop;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.controller.Controller;
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+
+@TeleOp(name = "Drivebase Only", group = "OpModes")
+public class Drivebase extends OpMode {
+    //turbo mode
+    public static double normal = 1;
+    public static double turbo = 0.5;
+    //create robot instance
+    private Robot robot;
+    //create controller 1 (driver)
+    private Controller controller1;
+    @Override
+    public void init() {
+        this.robot = new Robot(hardwareMap);
+        controller1 = new Controller(gamepad1);
+        telemetry.addLine("Started");
+        //update to make sure we receive last line of code
+        telemetry.update();
+    }
+
+    @Override
+    public void loop() {
+        //create and set X, Y, and Z for drive inputs
+        double x = -gamepad1.left_stick_y;
+        double y = -gamepad1.left_stick_x;
+        double z = -gamepad1.right_stick_x;
+        robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
+
+        //turbo activation
+        if (controller1.getA().isJustPressed()){
+            x *= turbo;
+            y *= turbo;
+            z *= turbo;
+        } else if (controller1.getA().isJustReleased()){
+            x *= normal;
+            y *= normal;
+            z *= normal;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java

@@ -0,0 +1,235 @@
+package org.firstinspires.ftc.teamcode.opmode.teleop;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.controller.Controller;
+import org.firstinspires.ftc.teamcode.hardware.Arm;
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+
+@Config
+@TeleOp(name = "Main TeleOp", group = "OpModes")
+public class MainTeleOp extends OpMode {
+
+    public static double normal = 0.7;
+    public static double turbo = 1;
+    public static double slow_mode = 0.35;
+    public static double intakeMax = 0.36;
+    public static double intakeMax2 = -0.70;
+
+    private Robot robot;
+    private Controller controller1;
+    private Controller controller2;
+
+    public boolean hang_counter = false;
+
+
+    private DcMotor[] motors = new DcMotor[8];
+    private Servo[] servos = new Servo[7];
+
+    @Override
+    public void init() {
+        this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
+        this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
+        this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
+        this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
+        this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
+        this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
+        this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
+        this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
+
+        this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
+        this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
+        this.servos[2] = this.hardwareMap.get(Servo.class, "Door");
+        this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
+        this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
+        this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
+        this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
+
+        controller1 = new Controller(gamepad1);
+        controller2 = new Controller(gamepad2);
+
+        this.robot = new Robot(hardwareMap);
+//        robot.intake.setpos(Intake.Position.STACK1);
+
+//        while (robot.camera.getFrameCount() < 1) {
+//            telemetry.addLine("Initializing...");
+//            telemetry.update();
+//        }
+
+        telemetry.addLine("Initialized");
+        this.telemetry = FtcDashboard.getInstance().getTelemetry();
+        telemetry.update();
+
+
+    }
+
+    double getBatteryVoltage() {
+        double result = Double.POSITIVE_INFINITY;
+        for (VoltageSensor sensor : hardwareMap.voltageSensor) {
+            double voltage = sensor.getVoltage();
+            if (voltage > 0) {
+                result = Math.min(result, voltage);
+            }
+        }
+        return result;
+    }
+
+    @Override
+    public void loop() {
+        for (DcMotor motor : this.motors) {
+            this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
+        }
+        for (Servo servo : this.servos) {
+            this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
+        }
+
+        this.telemetry.addData("battery", getBatteryVoltage());
+        this.telemetry.update();
+
+        // Driver 1
+        double x = -gamepad1.left_stick_y;
+        double y = -gamepad1.left_stick_x;
+        double z = -gamepad1.right_stick_x;
+
+        if (controller1.getRightTrigger().getValue() > 0.1) {
+            x *= turbo;
+            y *= turbo;
+            z *= turbo;
+        }
+        else if (controller1.getLeftTrigger().getValue() > 0.1) {
+            x *= slow_mode;
+            y *= slow_mode;
+            z *= slow_mode;
+        } else {
+            x *= normal;
+            y *= normal;
+            z *= normal;
+        }
+        robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
+        // Drone launcher
+        if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+            this.robot.endGameMechs.launch();
+        } else {
+            this.robot.endGameMechs.reset();
+        }
+
+//        if (controller1.getRightBumper().isPressed()) {
+//            this.robot.endGameMechs.Finger_in();
+//        }else {
+//            this.robot.endGameMechs.Finger_out();
+//        }
+        //Hang Motor
+
+//        if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
+//            this.robot.endGameMechs.hang_init_pos();
+//            hang_counter = true;
+//        }
+//        else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){
+//            this.robot.endGameMechs.hang_final_pos();
+//            hang_counter = false;
+//        }
+
+
+        if (controller1.getB().isPressed()) {
+            this.robot.endGameMechs.hang_final_pos();
+        } else  {
+            this.robot.endGameMechs.hang_init_pos();
+        }
+
+        if (controller1.getX().isPressed()) {
+            this.robot.intake.wheel_spit();
+        } else if (controller1.getY().isPressed()) {
+            this.robot.intake.wheel_swallow();
+        } else {
+            this.robot.intake.wheel_stop();
+        }
+
+
+        // Driver 2
+        if (controller2.getRightTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
+        }
+        else if(controller2.getLeftTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
+        }
+        else {
+            robot.intake.setDcMotor(0);
+        }
+
+        if (controller2.getRightBumper().isJustPressed()) {
+            robot.intake.incrementPos();
+        }
+        if (controller2.getLeftBumper().isJustPressed()) {
+            robot.intake.decrementPos();
+        }
+
+
+
+
+
+        // macros
+        switch (robot.runningMacro) {
+            case (0): // manual mode
+
+                robot.slides.increaseTarget(controller2.getLeftStick().getY());
+                if (robot.intake.getPower() >= 0.01) {
+                    robot.arm.setDoor(OPEN);
+                } else if (robot.intake.getPower() <= -0.01) {
+                    robot.arm.setDoor(OPEN);
+                } else if (controller2.getLeftBumper().isPressed()) {
+                    robot.arm.setDoor(Arm.DoorPosition.OPEN);
+                } else {
+                    robot.arm.setDoor(CLOSE);
+                }
+
+                if (controller2.getX().isJustPressed()) {
+                    robot.runningMacro = 1;
+                } else if (controller2.getY().isJustPressed()) {
+                    robot.runningMacro = 2;
+                } else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 3;
+                } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 4;
+                } else if (controller2.getDDown().isJustPressed() ) {
+                    robot.runningMacro = 5;
+                }
+
+                break;
+            case (1):
+                robot.extendMacro(Slides.tier1, getRuntime());
+                break;
+            case (2):
+                robot.extendMacro(Slides.tier2, getRuntime());
+                break;
+            case (3):
+                robot.extendMacro(Slides.tier3, getRuntime());
+                break;
+            case (4):
+                robot.resetMacro(0, getRuntime());
+                break;
+            case(5):
+                robot.extendMacro(Slides.mini_tier1, getRuntime());
+                break;
+        }
+
+        // update and telemetry
+        robot.update(getRuntime());
+        controller1.update();
+        controller2.update();
+        telemetry.addLine(robot.getTelemetry());
+        telemetry.update();
+
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java

@@ -0,0 +1,250 @@
+package org.firstinspires.ftc.teamcode.opmode.teleop;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.controller.Controller;
+import org.firstinspires.ftc.teamcode.hardware.Arm;
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+
+@Config
+@TeleOp(name = "Red TeleOp", group = "OpModes")
+public class RedTeleop extends OpMode {
+
+    public static double normal = 0.7;
+    public static double turbo = 1;
+    public static double slow_mode = 0.35;
+    public static double intakeMax = 0.36;
+    public static double intakeMax2 = -0.70;
+
+    private Robot robot;
+    private Controller controller1;
+    private Controller controller2;
+
+    public boolean hang_counter = false;
+
+
+    private DcMotor[] motors = new DcMotor[8];
+    private Servo[] servos = new Servo[7];
+
+    @Override
+    public void init() {
+        this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
+        this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
+        this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
+        this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
+        this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
+        this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
+        this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
+        this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
+
+        this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
+        this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
+        this.servos[2] = this.hardwareMap.get(Servo.class, "Door");
+        this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
+        this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
+        this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
+        this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
+
+        controller1 = new Controller(gamepad1);
+        controller2 = new Controller(gamepad2);
+
+        this.robot = new Robot(hardwareMap);
+//        robot.intake.setpos(Intake.Position.STACK1);
+
+//        while (robot.camera.getFrameCount() < 1) {
+//            telemetry.addLine("Initializing...");
+//            telemetry.update();
+//        }
+
+        telemetry.addLine("Initialized");
+        this.telemetry = FtcDashboard.getInstance().getTelemetry();
+        telemetry.update();
+        this.robot.alliance = 2;
+
+        this.robot.leds.setPattern(79);
+
+
+    }
+
+    double getBatteryVoltage() {
+        double result = Double.POSITIVE_INFINITY;
+        for (VoltageSensor sensor : hardwareMap.voltageSensor) {
+            double voltage = sensor.getVoltage();
+            if (voltage > 0) {
+                result = Math.min(result, voltage);
+            }
+        }
+        return result;
+    }
+
+    @Override
+    public void loop() {
+        for (DcMotor motor : this.motors) {
+            this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
+        }
+        for (Servo servo : this.servos) {
+            this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
+        }
+
+        this.telemetry.addData("battery", getBatteryVoltage());
+        this.telemetry.update();
+
+        this.robot.leds.setPattern(80);
+
+        // Driver 1
+        double x = -gamepad1.left_stick_y;
+        double y = -gamepad1.left_stick_x;
+        double z = -gamepad1.right_stick_x;
+
+        if (controller1.getRightTrigger().getValue() > 0.1) {
+            x *= turbo;
+            y *= turbo;
+            z *= turbo;
+        }
+        else if (controller1.getLeftTrigger().getValue() > 0.1) {
+            x *= slow_mode;
+            y *= slow_mode;
+            z *= slow_mode;
+        } else {
+            x *= normal;
+            y *= normal;
+            z *= normal;
+        }
+        robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
+        // Drone launcher
+        if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+            this.robot.endGameMechs.launch();
+        } else {
+            this.robot.endGameMechs.reset();
+        }
+
+//        if (controller1.getRightBumper().isPressed()) {
+//            this.robot.endGameMechs.Finger_in();
+//        }else {
+//            this.robot.endGameMechs.Finger_out();
+//        }
+        //Hang Motor
+
+//        if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
+//            this.robot.endGameMechs.hang_init_pos();
+//            hang_counter = true;
+//        }
+//        else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){
+//            this.robot.endGameMechs.hang_final_pos();
+//            hang_counter = false;
+//        }
+
+
+        if (controller1.getB().isPressed()) {
+            this.robot.endGameMechs.hang_final_pos();
+        } else  {
+            this.robot.endGameMechs.hang_init_pos();
+        }
+
+        if (controller1.getX().isPressed()) {
+            this.robot.intake.wheel_spit();
+        } else if (controller1.getY().isPressed()) {
+            this.robot.intake.wheel_swallow();
+        } else {
+            this.robot.intake.wheel_stop();
+        }
+
+
+        // Driver 2
+        if (controller2.getRightTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
+        }
+        else if(controller2.getLeftTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
+        }
+        else {
+            robot.intake.setDcMotor(0);
+        }
+
+        if (controller2.getRightBumper().isJustPressed()) {
+            robot.intake.incrementPos();
+        }
+        if (controller2.getLeftBumper().isJustPressed()) {
+            robot.intake.decrementPos();
+        }
+
+
+
+
+
+        // macros
+        switch (robot.runningMacro) {
+            case (0): // manual mode
+
+                robot.slides.increaseTarget(controller2.getLeftStick().getY());
+
+//                if (robot.intake.getPower() >= 0.01) {
+//                    robot.arm.setDoor(OPEN);
+//                } else if (robot.intake.getPower() <= -0.01) {
+//                    robot.arm.setDoor(OPEN);
+//                } else if (controller2.getLeftBumper().isPressed()) {
+//                    robot.arm.setDoor(Arm.DoorPosition.OPEN);
+//                } else {
+//                    robot.arm.setDoor(CLOSE);
+//                }
+                //Elbowpos(
+                if (controller2.getDRight().isJustPressed()){
+                    robot.arm.setElbowPos(4);
+                } else if (controller2.getDLeft().isJustPressed()) {
+                    robot.arm.setElbowPos(3);
+                }else if(controller2.getDUp().isJustPressed())  {
+                    robot.arm.setElbowPos(1);
+                }
+
+
+                if (controller2.getX().isJustPressed()) {
+                    robot.runningMacro = 1;
+                } else if (controller2.getY().isJustPressed()) {
+                    robot.runningMacro = 2;
+                } else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 3;
+                } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 4;
+                } else if (controller2.getDDown().isJustPressed() ) {
+                    robot.runningMacro = 5;
+                }
+
+                break;
+            case (1):
+                robot.extendMacro(Slides.tier1, getRuntime());
+                break;
+            case (2):
+                robot.extendMacro(Slides.tier2, getRuntime());
+                break;
+            case (3):
+                robot.extendMacro(Slides.tier3, getRuntime());
+                break;
+            case (4):
+                robot.resetMacro(0, getRuntime());
+                break;
+            case(5):
+                robot.extendMacro(Slides.mini_tier1, getRuntime());
+                break;
+        }
+
+        // update and telemetry
+        robot.update(getRuntime());
+        controller1.update();
+        controller2.update();
+        telemetry.addLine(robot.getTelemetry());
+        telemetry.update();
+
+
+    }
+}

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

@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive;
+
+import com.acmerobotics.dashboard.config.Config;
+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.889765; // in
+    public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
+    public static double TRACK_WIDTH = 13.5; // in
+
+    /*
+     * These are the feedforward parameters used to model the drive motor behavior. If you are using
+     * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
+     * motor encoders or have elected not to use them for velocity control, these values should be
+     * empirically tuned.
+     */
+//    public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
+
+    public static double kV = 0.012;
+    public static double kA = 0.004;
+    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 = 50;
+    public static double MAX_ACCEL = 50;
+    public static double MAX_ANG_VEL = 15;
+    public static double MAX_ANG_ACCEL = 8;
+
+    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/roadrunner/drive/MouseOdometry.java

@@ -0,0 +1,5 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive;//package org.firstinspires.ftc.teamcode.roadrunner.drive;
+//import java.awt.
+//
+//public class MouseOdometry {
+//}

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

@@ -0,0 +1,329 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.MecanumDrive;
+import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.roadrunner.util.AxisDirection;
+import org.firstinspires.ftc.teamcode.roadrunner.util.BNO055IMUUtil;
+import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+@Config
+public class SampleMecanumDrive extends MecanumDrive {
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(7, 0, 1);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(6.8, 0, 0);
+
+    public static double LATERAL_MULTIPLIER = 1;
+
+    public static double VX_WEIGHT = 1;
+    public static double VY_WEIGHT = 1;
+    public static double OMEGA_WEIGHT = 1;
+
+    private TrajectorySequenceRunner trajectorySequenceRunner;
+
+    private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH);
+    private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL);
+
+    private TrajectoryFollower follower;
+
+    private DcMotorEx leftFront, leftRear, rightRear, rightFront;
+    private List<DcMotorEx> motors;
+
+    private BNO055IMU imu;
+    private VoltageSensor batteryVoltageSensor;
+
+    public SampleMecanumDrive(HardwareMap hardwareMap) {
+        super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER);
+
+        follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
+                new Pose2d(0.5, 0.5, Math.toRadians(1)), 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(BNO055IMU.class, "imu");
+        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
+        imu.initialize(parameters);
+
+        // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
+        // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
+        //
+        //             | +Z axis
+        //             |
+        //             |
+        //             |
+        //      _______|_____________     +Y axis
+        //     /       |_____________/|__________
+        //    /   REV / EXPANSION   //
+        //   /       / HUB         //
+        //  /_______/_____________//
+        // |_______/_____________|/
+        //        /
+        //       / +X axis
+        //
+        // This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
+        // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
+        //
+        // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
+         BNO055IMUUtil.remapZAxis(imu, AxisDirection.POS_X);
+
+
+        rightRear = hardwareMap.get(DcMotorEx.class, "BackRight");
+        rightFront = hardwareMap.get(DcMotorEx.class, "FrontRight");
+        leftFront = hardwareMap.get(DcMotorEx.class, "FrontLeft");
+        leftRear = hardwareMap.get(DcMotorEx.class, "BackLeft");
+
+        motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+
+        for (DcMotorEx motor : motors) {
+            MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+            motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+            motor.setMotorType(motorConfigurationType);
+        }
+
+
+        if (DriveConstants.RUN_USING_ENCODER) {
+            setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        }
+
+        setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        if (DriveConstants.RUN_USING_ENCODER && DriveConstants.MOTOR_VELO_PID != null) {
+            setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID);
+        }
+
+        // TODO: reverse any motors using DcMotor.setDirection()
+      rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
+      rightRear.setDirection(DcMotorSimple.Direction.REVERSE);
+      leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+      leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // TODO: if desired, use setLocalizer() to change the localization method
+        // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
+
+        setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
+//        setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap));
+
+        trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+        return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+        return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+        return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+    }
+
+    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+        return new TrajectorySequenceBuilder(
+                startPose,
+                VEL_CONSTRAINT, ACCEL_CONSTRAINT,
+                DriveConstants.MAX_ANG_VEL, DriveConstants.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() {
+        List<Double> wheelPositions = new ArrayList<>();
+        for (DcMotorEx motor : motors) {
+            wheelPositions.add(DriveConstants.encoderTicksToInches(motor.getCurrentPosition()));
+        }
+        return wheelPositions;
+    }
+
+    @Override
+    public List<Double> getWheelVelocities() {
+        List<Double> wheelVelocities = new ArrayList<>();
+        for (DcMotorEx motor : motors) {
+            wheelVelocities.add(DriveConstants.encoderTicksToInches(motor.getVelocity()));
+        }
+        return wheelVelocities;
+    }
+
+    @Override
+    public void setMotorPowers(double v, double v1, double v2, double v3) {
+        leftFront.setPower(v);
+        leftRear.setPower(v1);
+        rightRear.setPower(-v2);
+        rightFront.setPower(-v3);
+    }
+
+    @Override
+    public double getRawExternalHeading() {
+        return imu.getAngularOrientation().firstAngle;
+    }
+
+    @Override
+    public Double getExternalHeadingVelocity() {
+        // To work around an SDK bug, use -zRotationRate in place of xRotationRate
+        // and -xRotationRate in place of zRotationRate (yRotationRate behaves as 
+        // expected). This bug does NOT affect orientation. 
+        //
+        // See https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251 for details.
+        return (double) -imu.getAngularVelocity().zRotationRate;
+    }
+
+    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+        return new MinVelocityConstraint(Arrays.asList(
+                new AngularVelocityConstraint(maxAngularVel),
+                new MecanumVelocityConstraint(maxVel, trackWidth)
+        ));
+    }
+
+    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+        return new ProfileAccelerationConstraint(maxAccel);
+    }
+
+    public String getTelemetry() {
+        return "Drive: "+getPoseEstimate();
+    }
+}

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

@@ -0,0 +1,82 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive;
+
+import androidx.annotation.NonNull;
+
+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.roadrunner.util.Encoder;
+
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |
+ *    | ||        || |
+ *    | ||        || |
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *
+ */
+public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
+    public static double TICKS_PER_REV = 8192;
+    public static double WHEEL_RADIUS = 0.6889764; // in
+    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+
+    public static double LATERAL_DISTANCE = 11.686023622; // in; distance between the left and right wheels
+    public static double FORWARD_OFFSET = 4.89812992126; // in; offset of the lateral wheel
+
+    private Encoder leftEncoder, rightEncoder, frontEncoder;
+
+    public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
+        super(Arrays.asList(
+                new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
+                new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
+                new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
+        ));
+
+        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
+        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
+        frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight"));
+
+        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+        leftEncoder.setDirection(Encoder.Direction.REVERSE);
+//        rightEncoder.setDirection(Encoder.Direction.REVERSE);
+//        frontEncoder.setDirection(Encoder.Direction.REVERSE);
+    }
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        return Arrays.asList(
+                encoderTicksToInches(leftEncoder.getCurrentPosition()),
+                encoderTicksToInches(rightEncoder.getCurrentPosition()),
+                encoderTicksToInches(frontEncoder.getCurrentPosition())
+        );
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelVelocities() {
+        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
+        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
+        //  compensation method
+
+        return Arrays.asList(
+                encoderTicksToInches(leftEncoder.getCorrectedVelocity()),
+                encoderTicksToInches(rightEncoder.getCorrectedVelocity()),
+                encoderTicksToInches(frontEncoder.getCorrectedVelocity())
+        );
+    }
+}

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

@@ -0,0 +1,108 @@
+package org.firstinspires.ftc.teamcode.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.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 = -1.5; // X is the up and down direction
+    public static double PARALLEL_Y = 7.25; // Y is the strafe direction
+
+    public static double PERPENDICULAR_X = -6.1;
+    public static double PERPENDICULAR_Y = 0;
+
+    public static double MULTIPLIER_X = 1.0;
+    public static double MULTIPLIER_Y = 1.0;
+
+    // Parallel/Perpendicular to the forward axis
+    // Parallel wheel is parallel to the forward axis
+    // Perpendicular is perpendicular to the forward axis
+    private Encoder parallelEncoder, perpendicularEncoder;
+
+    private SampleMecanumDrive drive;
+
+    public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, SampleMecanumDrive 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, "BackLeft"));
+//        parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
+        perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "FrontLeft"));
+
+        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+        perpendicularEncoder.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()),
+                encoderTicksToInches(perpendicularEncoder.getCurrentPosition())
+        );
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelVelocities() {
+        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
+        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
+        //  compensation method
+
+        return Arrays.asList(
+                encoderTicksToInches(parallelEncoder.getCorrectedVelocity()),
+                encoderTicksToInches(perpendicularEncoder.getCorrectedVelocity())
+        );
+    }
+}

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

@@ -0,0 +1,203 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_RPM;
+import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.rpmToVelocity;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+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.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.roadrunner.util.LoggingUtil;
+
+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 = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        NanoClock clock = NanoClock.system();
+
+        telemetry.addLine("Press play to begin the feedforward tuning routine");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Would you like to fit kStatic?");
+        telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+        telemetry.update();
+
+        boolean fitIntercept = false;
+        while (!isStopRequested()) {
+            if (gamepad1.y) {
+                fitIntercept = true;
+                while (!isStopRequested() && gamepad1.y) {
+                    idle();
+                }
+                break;
+            } else if (gamepad1.b) {
+                while (!isStopRequested() && gamepad1.b) {
+                    idle();
+                }
+                break;
+            }
+            idle();
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine(Misc.formatInvariant(
+                "Place your robot on the field with at least %.2f in of room in front", DISTANCE));
+        telemetry.addLine("Press (Y/Δ) to begin");
+        telemetry.update();
+
+        while (!isStopRequested() && !gamepad1.y) {
+            idle();
+        }
+        while (!isStopRequested() && gamepad1.y) {
+            idle();
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        double maxVel = rpmToVelocity(MAX_RPM);
+        double finalVel = MAX_POWER * maxVel;
+        double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
+        double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
+
+        List<Double> timeSamples = new ArrayList<>();
+        List<Double> positionSamples = new ArrayList<>();
+        List<Double> powerSamples = new ArrayList<>();
+
+        drive.setPoseEstimate(new Pose2d());
+
+        double startTime = clock.seconds();
+        while (!isStopRequested()) {
+            double elapsedTime = clock.seconds() - startTime;
+            if (elapsedTime > rampTime) {
+                break;
+            }
+            double vel = accel * elapsedTime;
+            double power = vel / maxVel;
+
+            timeSamples.add(elapsedTime);
+            positionSamples.add(drive.getPoseEstimate().getX());
+            powerSamples.add(power);
+
+            drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
+            drive.updatePoseEstimate();
+        }
+        drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+        telemetry.clearAll();
+        telemetry.addLine("Quasi-static ramp up test complete");
+        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));
+
+
+            telemetry.clearAll();
+            telemetry.addLine("Constant power test complete");
+            telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
+            telemetry.update()));
+        }
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}

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

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

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

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

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

@@ -0,0 +1,56 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+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.roadrunner.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.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
+
+@Autonomous(group = "drive")
+public class FollowerPIDTuner extends LinearOpMode {
+    public static double DISTANCE = 36; // in
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
+
+        drive.setPoseEstimate(startPose);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (!isStopRequested()) {
+            TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .forward(DISTANCE)
+                    .turn(Math.toRadians(90))
+                    .build();
+            drive.followTrajectorySequence(trajSeq);
+        }
+    }
+}

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

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

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

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

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

@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+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.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
+ * <p>
+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds.
+ * <p>
+ * Further fine tuning of MAX_ANG_VEL may be desired.
+ */
+
+//@Config
+@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 {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        telemetry = new MultipleTelemetry(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.update();
+
+        while (!isStopRequested()) idle();
+    }
+}

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

@@ -0,0 +1,83 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+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.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.teamcode.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum velocity your bot can achieve under load. It
+ * will also calculate the effective kF value for your velocity PID.
+ * <p>
+ * Upon pressing start, your bot will run at max power for RUNTIME seconds.
+ * <p>
+ * Further fine tuning of kF may be desired.
+ */
+//@Config
+@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 {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        telemetry = new MultipleTelemetry(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("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/roadrunner/drive/opmode/MotorDirectionDebugger.java

@@ -0,0 +1,90 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+
+/**
+ * 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.
+ */
+//@Config
+@TeleOp(group = "drive")
+public class MotorDirectionDebugger extends LinearOpMode {
+    public static double MOTOR_POWER = 0.7;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        telemetry.addLine("Press play to begin the debugging opmode");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+
+        while (!isStopRequested()) {
+            telemetry.addLine("Press each button to turn on its respective motor");
+            telemetry.addLine();
+            telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
+            telemetry.addLine();
+
+            if(gamepad1.x) {
+                drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
+                telemetry.addLine("Running Motor: Front Left");
+            } else if(gamepad1.y) {
+                drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
+                telemetry.addLine("Running Motor: Front Right");
+            } else if(gamepad1.b) {
+                drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
+                telemetry.addLine("Running Motor: Rear Right");
+            } else if(gamepad1.a) {
+                drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
+                telemetry.addLine("Running Motor: Rear Left");
+            } else {
+                drive.setMotorPowers(0, 0, 0, 0);
+                telemetry.addLine("Running Motor: None");
+            }
+
+            telemetry.update();
+        }
+    }
+}

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

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

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

@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.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.roadrunner.drive.SampleMecanumDrive;
+
+/*
+ * 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 {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        Trajectory traj = drive.trajectoryBuilder(new Pose2d())
+                .splineTo(new Vector2d(30, 30), 0)
+                .build();
+
+        drive.followTrajectory(traj);
+
+        sleep(2000);
+
+        drive.followTrajectory(
+                drive.trajectoryBuilder(traj.end(), true)
+                        .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+                        .build()
+        );
+    }
+}

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

@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+
+/*
+ * 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 = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+                .strafeRight(DISTANCE)
+                .build();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.followTrajectory(trajectory);
+
+        Pose2d poseEstimate = drive.getPoseEstimate();
+        telemetry.addData("finalX", poseEstimate.getX());
+        telemetry.addData("finalY", poseEstimate.getY());
+        telemetry.addData("finalHeading", poseEstimate.getHeading());
+        telemetry.update();
+
+        while (!isStopRequested() && opModeIsActive()) ;
+    }
+}

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

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

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

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

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

@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+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.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.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 = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+        }
+
+        telemetry.addLine("Press play to begin the forward offset tuner");
+        telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.addLine("Running...");
+        telemetry.update();
+
+        MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
+        for (int i = 0; i < NUM_TRIALS; i++) {
+            drive.setPoseEstimate(new Pose2d());
+
+            // it is important to handle heading wraparounds
+            double headingAccumulator = 0;
+            double lastHeading = 0;
+
+            drive.turnAsync(Math.toRadians(ANGLE));
+
+            while (!isStopRequested() && drive.isBusy()) {
+                double heading = drive.getPoseEstimate().getHeading();
+                headingAccumulator += Angle.norm(heading - lastHeading);
+                lastHeading = heading;
+
+                drive.update();
+            }
+
+            double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
+                    drive.getPoseEstimate().getY() / headingAccumulator;
+            forwardOffsetStats.add(forwardOffset);
+
+            sleep(DELAY);
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Tuning complete");
+        telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
+                forwardOffsetStats.getMean(),
+                forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+        telemetry.update();
+
+        while (!isStopRequested()) {
+            idle();
+        }
+    }
+}

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

@@ -0,0 +1,131 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+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.roadrunner.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.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 {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+            RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+                    + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+                    + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+        }
+
+        telemetry.addLine("Prior to beginning the routine, please read the directions "
+                + "located in the comments of the opmode file.");
+        telemetry.addLine("Press play to begin the tuning routine.");
+        telemetry.addLine("");
+        telemetry.addLine("Press Y/△ to stop the routine.");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        telemetry.clearAll();
+        telemetry.update();
+
+        double headingAccumulator = 0;
+        double lastHeading = 0;
+
+        boolean tuningFinished = false;
+
+        while (!isStopRequested() && !tuningFinished) {
+            Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
+            drive.setDrivePower(vel);
+
+            drive.update();
+
+            double heading = drive.getPoseEstimate().getHeading();
+            double deltaHeading = heading - lastHeading;
+
+            headingAccumulator += Angle.normDelta(deltaHeading);
+            lastHeading = heading;
+
+            telemetry.clearAll();
+            telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
+            telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
+            telemetry.addLine();
+            telemetry.addLine("Press Y/△ to conclude routine");
+            telemetry.update();
+
+            if (gamepad1.y)
+                tuningFinished = true;
+        }
+
+        telemetry.clearAll();
+        telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
+        telemetry.addLine("Effective LATERAL_DISTANCE: " +
+                (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
+
+        telemetry.update();
+
+        while (!isStopRequested()) idle();
+    }
+}

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

@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
+
+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.roadrunner.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test turning capabilities.
+ */
+//@Config
+@Disabled
+@Autonomous(group = "drive")
+public class TurnTest extends LinearOpMode {
+    public static double ANGLE = 90; // deg
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive.turn(Math.toRadians(ANGLE));
+    }
+}

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

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

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

@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+import org.firstinspires.ftc.teamcode.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/roadrunner/trajectorysequence/TrajectorySequenceBuilder.java

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

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

@@ -0,0 +1,271 @@
+package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.NanoClock;
+
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.WaitSegment;
+import org.firstinspires.ftc.teamcode.roadrunner.util.DashboardUtil;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.LinkedList;
+import java.util.List;
+
+public class TrajectorySequenceRunner {
+    public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
+    public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
+    public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
+
+    public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
+    public static String COLOR_ACTIVE_TURN = "#7c4dff";
+    public static String COLOR_ACTIVE_WAIT = "#dd2c00";
+
+    public static int POSE_HISTORY_LIMIT = 100;
+
+    private final TrajectoryFollower follower;
+
+    private final PIDFController turnController;
+
+    private final NanoClock clock;
+
+    private TrajectorySequence currentTrajectorySequence;
+    private double currentSegmentStartTime;
+    private int currentSegmentIndex;
+    private int lastSegmentIndex;
+
+    private Pose2d lastPoseError = new Pose2d();
+
+    List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
+
+    private final FtcDashboard dashboard;
+    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
+
+    public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) {
+        this.follower = follower;
+
+        turnController = new PIDFController(headingPIDCoefficients);
+        turnController.setInputBounds(0, 2 * Math.PI);
+
+        clock = NanoClock.system();
+
+        dashboard = FtcDashboard.getInstance();
+        dashboard.setTelemetryTransmissionInterval(25);
+    }
+
+    public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+        currentTrajectorySequence = trajectorySequence;
+        currentSegmentStartTime = clock.seconds();
+        currentSegmentIndex = 0;
+        lastSegmentIndex = -1;
+    }
+
+    public @Nullable
+    DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
+        Pose2d targetPose = null;
+        DriveSignal driveSignal = null;
+
+        TelemetryPacket packet = new TelemetryPacket();
+        Canvas fieldOverlay = packet.fieldOverlay();
+
+        SequenceSegment currentSegment = null;
+
+        if (currentTrajectorySequence != null) {
+            if (currentSegmentIndex >= currentTrajectorySequence.size()) {
+                for (TrajectoryMarker marker : remainingMarkers) {
+                    marker.getCallback().onMarkerReached();
+                }
+
+                remainingMarkers.clear();
+
+                currentTrajectorySequence = null;
+            }
+
+            if (currentTrajectorySequence == null)
+                return new DriveSignal();
+
+            double now = clock.seconds();
+            boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
+
+            currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
+
+            if (isNewTransition) {
+                currentSegmentStartTime = now;
+                lastSegmentIndex = currentSegmentIndex;
+
+                for (TrajectoryMarker marker : remainingMarkers) {
+                    marker.getCallback().onMarkerReached();
+                }
+
+                remainingMarkers.clear();
+
+                remainingMarkers.addAll(currentSegment.getMarkers());
+                Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
+            }
+
+            double deltaTime = now - currentSegmentStartTime;
+
+            if (currentSegment instanceof TrajectorySegment) {
+                Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+                if (isNewTransition)
+                    follower.followTrajectory(currentTrajectory);
+
+                if (!follower.isFollowing()) {
+                    currentSegmentIndex++;
+
+                    driveSignal = new DriveSignal();
+                } else {
+                    driveSignal = follower.update(poseEstimate, poseVelocity);
+                    lastPoseError = follower.getLastError();
+                }
+
+                targetPose = currentTrajectory.get(deltaTime);
+            } else if (currentSegment instanceof TurnSegment) {
+                MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
+
+                turnController.setTargetPosition(targetState.getX());
+
+                double correction = turnController.update(poseEstimate.getHeading());
+
+                double targetOmega = targetState.getV();
+                double targetAlpha = targetState.getA();
+
+                lastPoseError = new Pose2d(0, 0, turnController.getLastError());
+
+                Pose2d startPose = currentSegment.getStartPose();
+                targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
+
+                driveSignal = new DriveSignal(
+                        new Pose2d(0, 0, targetOmega + correction),
+                        new Pose2d(0, 0, targetAlpha)
+                );
+
+                if (deltaTime >= currentSegment.getDuration()) {
+                    currentSegmentIndex++;
+                    driveSignal = new DriveSignal();
+                }
+            } else if (currentSegment instanceof WaitSegment) {
+                lastPoseError = new Pose2d();
+
+                targetPose = currentSegment.getStartPose();
+                driveSignal = new DriveSignal();
+
+                if (deltaTime >= currentSegment.getDuration()) {
+                    currentSegmentIndex++;
+                }
+            }
+
+            while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
+                remainingMarkers.get(0).getCallback().onMarkerReached();
+                remainingMarkers.remove(0);
+            }
+        }
+
+        poseHistory.add(poseEstimate);
+
+        if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
+            poseHistory.removeFirst();
+        }
+
+        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/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.java

@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.java

@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/trajectorysequence/sequencesegment/TurnSegment.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/trajectorysequence/sequencesegment/WaitSegment.java

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/util/AssetsTrajectoryManager.java

@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/util/AxesSigns.java

@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/util/AxisDirection.java

@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/util/BNO055IMUUtil.java

@@ -0,0 +1,128 @@
+package org.firstinspires.ftc.teamcode.roadrunner.util;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+
+/**
+ * Various utility functions for the BNO055 IMU.
+ */
+public class BNO055IMUUtil {
+    /**
+     * Error for attempting an illegal remapping (lhs or multiple same axes)
+     */
+    public static class InvalidAxisRemapException extends RuntimeException {
+        public InvalidAxisRemapException(String detailMessage) {
+            super(detailMessage);
+        }
+    }
+
+    /**
+     * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
+     * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
+     * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
+     *
+     * Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
+     * Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
+     *
+     * NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate.
+     *
+     * @param imu IMU
+     * @param order axes order
+     * @param signs axes signs
+     */
+    public static void swapThenFlipAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
+        try {
+            // the indices correspond with the 2-bit axis encodings specified in the datasheet
+            int[] indices = order.indices();
+            // AxesSign's values align with the datasheet
+            int axisMapSigns = signs.bVal;
+
+            if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
+                throw new InvalidAxisRemapException("Same axis cannot be included in AxesOrder twice");
+            }
+
+            // ensure right-handed coordinate system
+            boolean isXSwapped = indices[0] != 0;
+            boolean isYSwapped = indices[1] != 1;
+            boolean isZSwapped = indices[2] != 2;
+            boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
+                    && (!isXSwapped || !isYSwapped || !isZSwapped);
+            boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
+            // != functions as xor
+            if (areTwoAxesSwapped != oddNumOfFlips) {
+                throw new InvalidAxisRemapException("Coordinate system is left-handed");
+            }
+
+            // Bit:  7  6 |  5  4  |  3  2  |  1  0  |
+            //   reserved | z axis | y axis | x axis |
+            int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
+
+            // Enter CONFIG mode
+            imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
+
+            Thread.sleep(100);
+
+            // Write the AXIS_MAP_CONFIG register
+            imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
+
+            // Write the AXIS_MAP_SIGN register
+            imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
+
+            // Switch back to the previous mode
+            imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
+
+            Thread.sleep(100);
+        } catch (InterruptedException e) {
+            Thread.currentThread().interrupt();
+        }
+    }
+
+    /**
+     * Remaps the IMU coordinate system so that the remapped +Z faces the provided
+     * {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
+     *
+     * @param imu IMU
+     * @param direction axis direction
+     */
+    public static void remapZAxis(BNO055IMU imu, AxisDirection direction) {
+        switch (direction) {
+            case POS_X:
+                swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.NPP);
+                break;
+            case NEG_X:
+                swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.PPN);
+                break;
+            case POS_Y:
+                swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PNP);
+                break;
+            case NEG_Y:
+                swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PPN);
+                break;
+            case POS_Z:
+                swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PPP);
+                break;
+            case NEG_Z:
+                swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PNN);
+                break;
+        }
+    }
+
+    /**
+     * Now deprecated due to unintuitive parameter order.
+     * Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead.
+     *
+     * @param imu IMU
+     * @param order axes order
+     * @param signs axes signs
+     */
+    @Deprecated
+    public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
+        AxesOrder adjustedAxesOrder = order.reverse();
+        int[] indices = order.indices();
+        int axisSignValue = signs.bVal ^ (0b100 >> indices[0]);
+        AxesSigns adjustedAxesSigns = AxesSigns.fromBinaryValue(axisSignValue);
+
+        swapThenFlipAxes(imu, adjustedAxesOrder, adjustedAxesSigns);
+    }
+}

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

@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/util/Encoder.java

@@ -0,0 +1,98 @@
+package org.firstinspires.ftc.teamcode.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) {
+        double real = input;
+        while (Math.abs(estimate - real) > CPS_STEP / 2.0) {
+            real += Math.signum(estimate - real) * 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 double velocityEstimate;
+    private double lastUpdateTime;
+
+    public Encoder(DcMotorEx motor, NanoClock clock) {
+        this.motor = motor;
+        this.clock = clock;
+
+        this.direction = Direction.FORWARD;
+
+        this.lastPosition = 0;
+        this.velocityEstimate = 0.0;
+        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;
+    }
+
+    public int getCurrentPosition() {
+        int multiplier = getMultiplier();
+        int currentPosition = motor.getCurrentPosition() * multiplier;
+        if (currentPosition != lastPosition) {
+            double currentTime = clock.seconds();
+            double dt = currentTime - lastUpdateTime;
+            velocityEstimate = (currentPosition - lastPosition) / dt;
+            lastPosition = currentPosition;
+            lastUpdateTime = currentTime;
+        }
+        return currentPosition;
+    }
+
+    public double getRawVelocity() {
+        int multiplier = getMultiplier();
+        return motor.getVelocity() * multiplier;
+    }
+
+    public double getCorrectedVelocity() {
+        return inverseOverflow(getRawVelocity(), velocityEstimate);
+    }
+}

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

@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.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/roadrunner/util/LynxModuleUtil.java

@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode.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/util/CameraPosition.java

@@ -0,0 +1,5 @@
+package org.firstinspires.ftc.teamcode.util;
+
+public enum CameraPosition {
+    FRONT, BACK
+}

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

@@ -0,0 +1,6 @@
+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,23 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import android.graphics.Color;
+
+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,89 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Size;
+
+public class Configurables {
+    // Robot Constants
+    public static double R_ARM_POWER = 0.2;
+    public static double R_ARM_SPEED = 20;
+    public static int R_ARM_DEFAULT_POS = 0;
+    public static int R_ARM_UP_POS = 221;
+    public static int R_ARM_ALMOST_DOWN_POS = 650;
+    public static int R_ARM_DOWN_POS = 750;
+    public static double R_CLAW_CLOSED = 0.13;
+    public static double R_CLAW_OPEN = 0.7;
+    public static double R_INTAKE_SPEED = 0.9;
+    public static double R_INTAKE_SHIELD_UP = 0.17;//0.05
+    public static double R_INTAKE_SHIELD_DOWN = 0.68;//0.95
+    public static double R_INTAKE_SHIELD_SPEED = 0.04;
+    public static double R_SHOOTER_GOAL_POWER = 0.66;
+    public static double R_SHOOTER_MID_GOAL_POWER = 0.54;
+    public static double R_SHOOTER_POWERSHOT_POWER = 0.57;
+    public static double R_PUSHER_CLOSED = 0.35;
+    public static double R_PUSHER_OPEN = 0.55;
+    public static double R_PUSHER_DELAY = 0.15;
+
+    // Auto Aim Constants
+    public static double AUTO_AIM_OFFSET_X = 5;
+    public static double AUTO_AIM_WAIT = 0.2;
+    public static PIDFCoefficients AUTO_AIM_PID = new PIDFCoefficients(0.009, 0.3, 0.0019, 0);
+    public static double AUTO_AIM_ACCEPTABLE_ERROR = 2;
+    public static double AUTO_AIM_MIN_POWER = 0.14;
+
+    // CV Color Threshold Constants
+    public static Color CAMERA_RED_GOAL_LOWER       = new Color(165, 80, 80);
+    public static Color CAMERA_RED_GOAL_UPPER       = new Color(15, 255, 255);
+    public static Color CAMERA_RED_POWERSHOT_LOWER  = new Color(165, 80, 80);
+    public static Color CAMERA_RED_POWERSHOT_UPPER  = new Color(15, 255, 255);
+    public static Color CAMERA_BLUE_GOAL_LOWER      = new Color(75, 40, 80);
+    public static Color CAMERA_BLUE_GOAL_UPPER      = new Color(120, 255, 255);
+    public static Color CAMERA_BLACK_GOAL_LOWER      = new Color(0, 0, 0);
+    public static Color CAMERA_BLACK_GOAL_UPPER      = new Color(360, 0, 0);
+    public static Color CAMERA_BLUE_POWERSHOT_LOWER = new Color(75, 30, 50);
+    public static Color CAMERA_BLUE_POWERSHOT_UPPER = new Color(120, 255, 255);
+    public static Color CAMERA_ORANGE_LOWER         = new Color(0, 70, 50);
+    public static Color CAMERA_ORANGE_UPPER         = new Color(60, 255, 255);
+    public static Color CAMERA_WHITE_LOWER          = new Color(0, 0, 40);
+    public static Color CAMERA_WHITE_UPPER          = new Color(180, 30, 255);
+
+    // CV Detection Constants
+    public static double CV_MIN_STARTERSTACK_AREA = 0;
+    public static double CV_MIN_STARTERSTACK_SINGLE_AREA = 0.08;
+    public static double CV_MIN_STARTERSTACK_QUAD_AREA = 1.3;
+    public static double CV_MIN_GOAL_AREA = 0;
+    public static double CV_MAX_GOAL_AREA = 0.3;
+    public static double CV_MIN_POWERSHOT_AREA = 0.001;
+    public static double CV_MAX_POWERSHOT_AREA = 0.05;
+    public static Rect CV_STARTERSTACK_LOCATION = new Rect(75, 50, 190, 90);
+    public static Point CV_POWERSHOT_OFFSET = new Point(-3, -20); // offset from the bottom left of the goal to the top right of the powershot box (for red)
+    public static Size CV_POWERSHOT_DIMENSIONS = new Size(100, 50);
+
+    public static Size CV_GOAL_PROPER_ASPECT = new Size(11, 8.5);
+    public static double CV_GOAL_PROPER_AREA = 1.25;
+    public static double CV_GOAL_ALLOWABLE_AREA_ERROR = 1;
+    public static double CV_GOAL_ALLOWABLE_SOLIDARITY_ERROR = 1;
+    public static double CV_GOAL_CUTOFF_Y_LINE = 65;
+    public static double CV_GOAL_PROPER_HEIGHT = 107;
+    public static double CV_GOAL_MIN_CONFIDENCE = 3;
+
+    public static Color CV_POWERSHOT_OFFSETS_RED = new Color(-40, -30, -19);
+    public static Color CV_POWERSHOT_OFFSETS_BLUE = new Color(40, 30, 19);
+
+    // Old CV Detection Constants
+    public static double CV_GOAL_SIDE_ALLOWABLE_Y_ERROR = 20;
+    public static double CV_GOAL_SIDE_ALLOWABLE_SIZE_ERROR = 100;
+    public static Size CV_GOAL_SIDE_ASPECT_RATIO = new Size(6.5,15.5);
+    public static double CV_GOAL_SIDE_ALLOWABLE_ASPECT_ERROR = 10;
+
+    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;
+}

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

@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import androidx.annotation.VisibleForTesting;
+
+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 WHEEL_FRONT_LEFT = "frontLeft";
+    public static final String WHEEL_FRONT_RIGHT = "frontRight";
+    public static final String WHEEL_BACK_LEFT = "backLeft";
+    public static final String WHEEL_BACK_RIGHT = "backRight";
+    public static final String WEBCAM_NAME = "FrontWebcam";
+    public static final String ARM = "wobbler";
+    public static final String CLAW = "claw";
+    public static final String INTAKE = "intake";
+    public static final String INTAKE_SECONDARY = "secondary";
+    public static final String INTAKE_SHIELD = "shield";
+    public static final String SHOOTER = "wheel";
+    public static final String PUSHER = "pusher";
+    public static final String STACK_WEBCAM = "Stack Webcam";
+    public static final String TARGETING_WEBCAM = "Targeting Webcam";
+    public static final String IMU_SENSOR = "imu";
+    public static final String lServo = "lServo";
+    public static final String rServo = "rServo";
+    public static final String droneLauncher = "droneLauncher";
+}

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

@@ -0,0 +1,102 @@
+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 color)
+        {
+            setColor(color);
+            setAntiAlias(true);
+            setStrokeCap(Paint.Cap.ROUND);
+        }
+    }
+}

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/AprilTagDetectionPipeline.java

@@ -0,0 +1,194 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import static org.firstinspires.ftc.teamcode.vision.OpenCVUtil.LEFT_BOUNDARY_APRILTAG;
+import static org.firstinspires.ftc.teamcode.vision.OpenCVUtil.RIGHT_BOUNDARY_APRILTAG;
+
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point;
+import org.opencv.core.Point3;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.apriltag.AprilTagDetection;
+import org.openftc.apriltag.AprilTagDetectorJNI;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+import java.util.ArrayList;
+
+public class AprilTagDetectionPipeline extends OpenCvPipeline {
+
+    double fx;
+    double fy;
+    double cx;
+    double cy;
+    double tagsize;
+    double tagsizeX;
+    double tagsizeY;
+    Mat cameraMatrix;
+    private float decimation;
+    private Mat grey = new Mat();
+    Scalar blue = new Scalar(7,197,235,255);
+    Scalar red = new Scalar(255,0,0,255);
+    Scalar green = new Scalar(0,255,0,255);
+    Scalar white = new Scalar(255,255,255,255);
+    private long nativeApriltagPtr;
+    private boolean needToSetDecimation;
+    private final Object decimationSync = new Object();
+    private final Object detectionsUpdateSync = new Object();
+    private ArrayList<org.openftc.apriltag.AprilTagDetection> detectionsUpdate = new ArrayList<>();
+    private ArrayList<org.openftc.apriltag.AprilTagDetection> aprilTagDetections = new ArrayList<>();
+
+    @Override
+    public Mat processFrame(Mat input)
+    {
+        // Convert to greyscale
+        Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY);
+
+        synchronized (decimationSync)
+        {
+            if(needToSetDecimation)
+            {
+                AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation);
+                needToSetDecimation = false;
+            }
+        }
+
+        // Run AprilTag
+        aprilTagDetections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy);
+
+        synchronized (detectionsUpdateSync)
+        {
+            detectionsUpdate = aprilTagDetections;
+        }
+
+        for(AprilTagDetection detection : aprilTagDetections)
+        {
+            Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);
+            drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix);
+            draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix);
+        }
+        Imgproc.line(input, new Point(LEFT_BOUNDARY_APRILTAG,0), new Point(LEFT_BOUNDARY_APRILTAG,800), green, 2);
+        Imgproc.line(input, new Point(RIGHT_BOUNDARY_APRILTAG,0), new Point(RIGHT_BOUNDARY_APRILTAG,800), green, 2);
+
+        return input;
+    }
+
+    Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY)
+    {
+        // The actual 2d points of the tag detected in the image
+        MatOfPoint2f points2d = new MatOfPoint2f(points);
+
+        // The 3d points of the tag in an 'ideal projection'
+        Point3[] arrayPoints3d = new Point3[4];
+        arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0);
+        arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0);
+        arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0);
+        arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0);
+        MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d);
+
+        // Using this information, actually solve for pose
+        Pose pose = new Pose();
+        Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false);
+
+        return pose;
+    }
+
+    void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
+    {
+        // The points in 3D space we wish to project onto the 2D image plane.
+        // The origin of the coordinate space is assumed to be in the center of the detection.
+        MatOfPoint3f axis = new MatOfPoint3f(
+                new Point3(0,0,0),
+                new Point3(length,0,0),
+                new Point3(0,length,0),
+                new Point3(0,0,-length)
+        );
+
+        // Project those points
+        MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
+        Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
+        Point[] projectedPoints = matProjectedPoints.toArray();
+
+        // Draw the marker!
+        Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness);
+        Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness);
+        Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness);
+
+        Imgproc.circle(buf, projectedPoints[0], thickness, white, -1);
+    }
+
+    void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
+    {
+        //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
+        //       [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
+
+        // The points in 3D space we wish to project onto the 2D image plane.
+        // The origin of the coordinate space is assumed to be in the center of the detection.
+        MatOfPoint3f axis = new MatOfPoint3f(
+                new Point3(-tagWidth/2, tagHeight/2,0),
+                new Point3( tagWidth/2, tagHeight/2,0),
+                new Point3( tagWidth/2,-tagHeight/2,0),
+                new Point3(-tagWidth/2,-tagHeight/2,0),
+                new Point3(-tagWidth/2, tagHeight/2,-length),
+                new Point3( tagWidth/2, tagHeight/2,-length),
+                new Point3( tagWidth/2,-tagHeight/2,-length),
+                new Point3(-tagWidth/2,-tagHeight/2,-length));
+
+        // Project those points
+        MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
+        Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
+        Point[] projectedPoints = matProjectedPoints.toArray();
+
+        // Pillars
+        for(int i = 0; i < 4; i++)
+        {
+            Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness);
+        }
+
+        // Base lines
+        //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness);
+        //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness);
+        //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness);
+        //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness);
+
+        // Top lines
+        Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness);
+        Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness);
+        Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness);
+        Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness);
+    }
+
+    public ArrayList<AprilTagDetection> getLatestDetections()
+    {
+        return aprilTagDetections;
+    }
+
+    public void setDecimation(float decimation)
+    {
+        synchronized (decimationSync)
+        {
+            this.decimation = decimation;
+            needToSetDecimation = true;
+        }
+    }
+
+    class Pose
+    {
+        Mat rvec;
+        Mat tvec;
+
+        public Pose(){
+            rvec = new Mat();
+            tvec = new Mat();
+        }
+
+        public Pose(Mat rvec, Mat tvec)
+        {
+            this.rvec = rvec;
+            this.tvec = tvec;
+        }
+    }
+}

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

@@ -0,0 +1,131 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+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;
+
+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;
+
+// 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 maxAreaFactor) {
+        this.maxSizePx = frameSize;
+        this.minAreaPx = frameSize.area() * minAreaFactor;
+        this.maxAreaPx = frameSize.area() * maxAreaFactor;
+    }
+
+    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.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/OpenCVUtil.java

@@ -0,0 +1,109 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import org.firstinspires.ftc.teamcode.util.Color;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfInt;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.RotatedRect;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.imgproc.Moments;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class OpenCVUtil {
+    public static Color YELLOW_LOWER = new Color(38, 25, 0);
+    public static Color YELLOW_UPPER = new Color(105, 300, 300);
+
+    public static int LEFT_BOUNDARY = -25;
+    public static int RIGHT_BOUNDARY = 25;
+
+    public static int LEFT_BOUNDARY_APRILTAG = 0;
+    public static int RIGHT_BOUNDARY_APRILTAG = 800;
+
+    // Draw a point
+    // Draw a point
+    public static void drawPoint(Mat img, Point point, Scalar color, int radiusPx) {
+        Imgproc.circle(img, point, radiusPx, 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 void drawAngledRect(Mat img, MatOfPoint contour, Scalar color, boolean fill) {
+        RotatedRect rect = Imgproc.minAreaRect(new MatOfPoint2f(contour.toArray()));
+        Point[] vertices = new Point[4];
+        rect.points(vertices);
+        List<MatOfPoint> boxContours = new ArrayList<>();
+        boxContours.add(new MatOfPoint(vertices));
+        Imgproc.drawContours(img, boxContours, 0, color, fill ? -1 : 2);
+    }
+}

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

@@ -0,0 +1,140 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_LOWER;
+import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_UPPER;
+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.DETECTION_AREA_MAX;
+import static org.firstinspires.ftc.teamcode.util.Configurables.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 android.graphics.Canvas;
+
+import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+import org.firstinspires.ftc.teamcode.util.ScalarRange;
+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 {
+
+    CenterStageCommon.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);
+
+        if (alliance == CenterStageCommon.Alliance.Red) {
+            red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2));
+        }
+
+        if (alliance == CenterStageCommon.Alliance.Blue) {
+            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(CenterStageCommon.Alliance alliance) {
+        this.alliance = alliance;
+    }
+
+    public CenterStageCommon.Alliance getAlliance() {
+        return this.alliance;
+    }
+}

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

@@ -0,0 +1,151 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLACK_GOAL_LOWER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_LOWER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_UPPER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_LOWER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_UPPER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MAX_GOAL_AREA;
+import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MIN_GOAL_AREA;
+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.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.firstinspires.ftc.teamcode.util.Color;
+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 TargetingPipeline extends OpenCvPipeline {
+    Mat blurred = new Mat();
+    Mat hsv = new Mat();
+    Mat redMask1 = new Mat();
+    Mat redMask2 = new Mat();
+    Mat redMask = new Mat();
+    Mat blueMask = new Mat();
+    Mat blackMask = new Mat();
+    Scalar redGoalLower1;
+    Scalar redGoalUpper1;
+    Scalar redGoalLower2;
+    Scalar redGoalUpper2;
+    Scalar blueGoalLower1;
+    Scalar blueGoalUpper1;
+    Scalar blueGoalLower2;
+    Scalar blueGoalUpper2;
+    Scalar blackGoalLower1;
+    Scalar blackGoalUpper1;
+    Scalar blackGoalLower2;
+    Scalar blackGoalUpper2;
+
+    private Detection red;
+    private Detection blue;
+    private Detection black;
+
+    // Init
+    @Override
+    public void init(Mat input) {
+        red = new Detection(input.size(), CV_MIN_GOAL_AREA);
+        blue = new Detection(input.size(), CV_MIN_GOAL_AREA);
+        black = new Detection(input.size(), CV_MIN_GOAL_AREA);
+    }
+
+    // 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);
+        updateBlack(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(CAMERA_RED_GOAL_LOWER.getH(), CAMERA_RED_GOAL_LOWER.getS(), CAMERA_RED_GOAL_LOWER.getV());
+        redGoalUpper1 = new Scalar(180, CAMERA_RED_GOAL_UPPER.getS(), CAMERA_RED_GOAL_UPPER.getV());
+        redGoalLower2 = new Scalar(0, CAMERA_RED_GOAL_LOWER.getS(), CAMERA_RED_GOAL_LOWER.getV());
+        redGoalUpper2 = new Scalar(CAMERA_RED_GOAL_UPPER.getH(), CAMERA_RED_GOAL_UPPER.getS(), CAMERA_RED_GOAL_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(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA);
+            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(CAMERA_BLUE_GOAL_LOWER.getH(), CAMERA_BLUE_GOAL_LOWER.getS(), CAMERA_BLUE_GOAL_LOWER.getV());
+        blueGoalUpper1 = new Scalar(180, CAMERA_BLUE_GOAL_UPPER.getS(), CAMERA_BLUE_GOAL_UPPER.getV());
+        blueGoalLower2 = new Scalar(0, CAMERA_BLUE_GOAL_LOWER.getS(), CAMERA_BLUE_GOAL_LOWER.getV());
+        blueGoalUpper2 = new Scalar(CAMERA_BLUE_GOAL_UPPER.getH(), CAMERA_BLUE_GOAL_UPPER.getS(), CAMERA_BLUE_GOAL_UPPER.getV());
+        Core.inRange(hsv, blueGoalLower1, blueGoalUpper1, blueMask);
+        Core.inRange(hsv, blueGoalLower2, blueGoalUpper2, 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(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA);
+            newDetection.setContour(contours.get(i));
+            newDetection.draw(input, BLUE);
+        }
+
+        blue.setContour(getLargestContour(contours));
+        blue.fill(input, BLUE);
+    }
+
+    private void updateBlack(Mat input) {
+        // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
+        blackGoalLower1 = new Scalar(CAMERA_BLACK_GOAL_LOWER.getH(), CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
+        blackGoalUpper1 = new Scalar(180, CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
+        blackGoalLower2 = new Scalar(0, CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
+        blackGoalUpper2 = new Scalar(CAMERA_BLACK_GOAL_LOWER.getH(), CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
+        Core.inRange(hsv, blackGoalLower1, blackGoalUpper1, blackMask);
+        Core.inRange(hsv, blackGoalLower2, blackGoalUpper2, blackMask);
+        Imgproc.erode(blackMask, blackMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+        Imgproc.dilate(blackMask, blackMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+
+        ArrayList<MatOfPoint> contours = new ArrayList<>();
+        Imgproc.findContours(blackMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
+        for (int i = 0; i < contours.size(); i++) {
+            Detection newDetection = new Detection(input.size(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA);
+            newDetection.setContour(contours.get(i));
+            newDetection.draw(input, BLACK);
+        }
+
+        black.setContour(getLargestContour(contours));
+        black.fill(input, BLACK);
+    }
+
+    public Detection getRed() {
+        return this.red;
+    }
+    public Detection getBlue() {
+        return this.blue;
+    }
+}

build.common.gradle

@@ -93,14 +93,14 @@ android {
             signingConfig signingConfigs.release
 
             ndk {
-                abiFilters "armeabi-v7a", "arm64-v8a"
+                abiFilters "armeabi-v7a"
             }
         }
         debug {
             debuggable true
             jniDebuggable true
             ndk {
-                abiFilters "armeabi-v7a", "arm64-v8a"
+                abiFilters "armeabi-v7a"
             }
         }
     }
@@ -121,5 +121,6 @@ android {
 }
 
 repositories {
+    mavenCentral()
 }
 

build.dependencies.gradle

@@ -1,21 +1,25 @@
 repositories {
     mavenCentral()
     google() // Needed for androidx
+    maven { url = 'https://maven.brott.dev/' }
 }
 
 dependencies {
-    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: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: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.15'
+    implementation project(':ielib-core')
+    implementation project(':ielib')
 }
 

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.2.2'
     }
 }
 

settings.gradle

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