Getting parking to work again as path 6b. Original path during competition was not working properly.

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

.github/workflows/static.yml

@@ -0,0 +1,43 @@
+# Simple workflow for deploying static content to GitHub Pages
+name: Deploy static content to Pages
+
+on:
+  # Runs on pushes targeting the default branch
+  push:
+    branches: ["master"]
+
+  # Allows you to run this workflow manually from the Actions tab
+  workflow_dispatch:
+
+# Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages
+permissions:
+  contents: read
+  pages: write
+  id-token: write
+
+# Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued.
+# However, do NOT cancel in-progress runs as we want to allow these production deployments to complete.
+concurrency:
+  group: "pages"
+  cancel-in-progress: false
+
+jobs:
+  # Single deploy job since we're just deploying
+  deploy:
+    environment:
+      name: github-pages
+      url: ${{ steps.deployment.outputs.page_url }}
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+      - name: Setup Pages
+        uses: actions/configure-pages@v5
+      - name: Upload artifact
+        uses: actions/upload-pages-artifact@v3
+        with:
+          # Upload entire repository
+          path: '.'
+      - name: Deploy to GitHub Pages
+        id: deployment
+        uses: actions/deploy-pages@v4

FtcRobotController/build.gradle

@@ -14,7 +14,11 @@ android {
         buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"'
     }
 
-    compileSdkVersion 29
+    buildFeatures {
+        buildConfig = true
+    }
+
+    compileSdkVersion 30
 
     compileOptions {
         sourceCompatibility JavaVersion.VERSION_1_8

FtcRobotController/src/main/AndroidManifest.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0" encoding="utf-8"?>
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
           xmlns:tools="http://schemas.android.com/tools"
-    android:versionCode="55"
-      android:versionName="10.0">
+    android:versionCode="57"
+          android:versionName="10.1.1">
 
   <uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />
 

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

@@ -58,6 +58,9 @@ import java.util.List;
  * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the robot, relative to the field origin.
  * This information is provided in the "robotPose" member of the returned "detection".
  *
+ * To learn about the Field Coordinate System that is defined for FTC (and used by this OpMode), see the FTC-DOCS link below:
+ * https://ftc-docs.firstinspires.org/en/latest/game_specific_resources/field_coordinate_system/field-coordinate-system.html
+ *
  * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
  * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
  */

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

@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2024 Phil Malone
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import android.util.Size;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.SortOrder;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor;
+import org.firstinspires.ftc.vision.opencv.ColorRange;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+import org.opencv.core.RotatedRect;
+
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions
+ *
+ * Unlike a "color sensor" which determines the color of an object in the field of view, this "color locator"
+ * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that match the requested color range.
+ * These blobs can be further filtered and sorted to find the one most likely to be the item the user is looking for.
+ *
+ * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process.
+ *   The ColorBlobLocatorProcessor process is created first, and then the VisionPortal is built to use this process.
+ *   The ColorBlobLocatorProcessor analyses the ROI and locates pixels that match the ColorRange to form a "mask".
+ *   The matching pixels are then collected into contiguous "blobs" of pixels.  The outer boundaries of these blobs are called its "contour".
+ *   For each blob, the process then creates the smallest possible rectangle "boxFit" that will fully encase the contour.
+ *   The user can then call getBlobs() to retrieve the list of Blobs, where each Blob contains the contour and the boxFit data.
+ *   Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest contours are listed first.
+ *
+ * To aid the user, a colored boxFit rectangle is drawn on the camera preview to show the location of each Blob
+ * The original Blob contour can also be added to the preview.  This is helpful when configuring the ColorBlobLocatorProcessor 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
+ */
+
+@Disabled
+@TeleOp(name = "Concept: Vision Color-Locator", group = "Concept")
+public class ConceptVisionColorLocator extends LinearOpMode
+{
+    @Override
+    public void runOpMode()
+    {
+        /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class.
+         * - Specify the color range you are looking for.  You can use a predefined color, or create you own color range
+         *     .setTargetColorRange(ColorRange.BLUE)                      // use a predefined color match
+         *       Available predefined colors are: RED, BLUE YELLOW GREEN
+         *     .setTargetColorRange(new ColorRange(ColorSpace.YCrCb,      // or define your own color match
+         *                                           new Scalar( 32, 176,  0),
+         *                                           new Scalar(255, 255, 132)))
+         *
+         * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search.
+         *     This can be the entire frame, or a sub-region defined using:
+         *     1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system.
+         *     Use one form of the ImageRegion class to define the ROI.
+         *         ImageRegion.entireFrame()
+         *         ImageRegion.asImageCoordinates(50, 50,  150, 150)  100x100 pixel square near the upper left corner
+         *         ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5)  50% width/height square centered on screen
+         *
+         * - Define which contours are included.
+         *     You can get ALL the contours, or you can skip any contours that are completely inside another contour.
+         *        .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY)  // return all contours
+         *        .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)            // exclude contours inside other contours
+         *        note: EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up areas of solid color.
+         *
+         * - turn the display of contours ON or OFF.  Turning this on helps debugging but takes up valuable CPU time.
+         *        .setDrawContours(true)
+         *
+         * - include any pre-processing of the image or mask before looking for Blobs.
+         *     There are some extra processing you can include to improve the formation of blobs.  Using these features requires
+         *     an understanding of how they may effect the final blobs.  The "pixels" argument sets the NxN kernel size.
+         *        .setBlurSize(int pixels)    Blurring an image helps to provide a smooth color transition between objects, and smoother contours.
+         *                                    The higher the number of pixels, the more blurred the image becomes.
+         *                                    Note:  Even "pixels" values will be incremented to satisfy the "odd number" requirement.
+         *                                    Blurring too much may hide smaller features.  A "pixels" size of 5 is good for a 320x240 image.
+         *        .setErodeSize(int pixels)   Erosion removes floating pixels and thin lines so that only substantive objects remain.
+         *                                    Erosion can grow holes inside regions, and also shrink objects.
+         *                                    "pixels" in the range of 2-4 are suitable for low res images.
+         *        .setDilateSize(int pixels)  Dilation makes objects more visible by filling in small holes, making lines appear thicker,
+         *                                    and making filled shapes appear larger. Dilation is useful for joining broken parts of an
+         *                                    object, such as when removing noise from an image.
+         *                                    "pixels" in the range of 2-4 are suitable for low res images.
+         */
+        ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder()
+                .setTargetColorRange(ColorRange.BLUE)         // use a predefined color match
+                .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)    // exclude blobs inside blobs
+                .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5))  // search central 1/4 of camera view
+                .setDrawContours(true)                        // Show contours on the Stream Preview
+                .setBlurSize(5)                               // Smooth the transitions between different colors in image
+                .build();
+
+        /*
+         * Build a vision portal to run the Color Locator process.
+         *
+         *  - Add the colorLocator process created above.
+         *  - Set the desired video resolution.
+         *      Since a high resolution will not improve this process, choose a lower resolution that is
+         *      supported by your camera.  This will improve overall performance and reduce latency.
+         *  - Choose your video source.  This may be
+         *      .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))  .....   for a webcam
+         *  or
+         *      .setCamera(BuiltinCameraDirection.BACK)    ... for a Phone Camera
+         */
+        VisionPortal portal = new VisionPortal.Builder()
+                .addProcessor(colorLocator)
+                .setCameraResolution(new Size(320, 240))
+                .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+                .build();
+
+        telemetry.setMsTransmissionInterval(50);   // Speed up telemetry updates, Just use for debugging.
+        telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+
+        // WARNING:  To be able to view the stream preview on the Driver Station, this code runs in INIT mode.
+        while (opModeIsActive() || opModeInInit())
+        {
+            telemetry.addData("preview on/off", "... Camera Stream\n");
+
+            // Read the current list
+            List<ColorBlobLocatorProcessor.Blob> blobs = colorLocator.getBlobs();
+
+            /*
+             * The list of Blobs can be filtered to remove unwanted Blobs.
+             *   Note:  All contours will be still displayed on the Stream Preview, but only those that satisfy the filter
+             *          conditions will remain in the current list of "blobs".  Multiple filters may be used.
+             *
+             * Use any of the following filters.
+             *
+             * ColorBlobLocatorProcessor.Util.filterByArea(minArea, maxArea, blobs);
+             *   A Blob's area is the number of pixels contained within the Contour.  Filter out any that are too big or small.
+             *   Start with a large range and then refine the range based on the likely size of the desired object in the viewfinder.
+             *
+             * ColorBlobLocatorProcessor.Util.filterByDensity(minDensity, maxDensity, blobs);
+             *   A blob's density is an indication of how "full" the contour is.
+             *   If you put a rubber band around the contour you would get the "Convex Hull" of the contour.
+             *   The density is the ratio of Contour-area to Convex Hull-area.
+             *
+             * ColorBlobLocatorProcessor.Util.filterByAspectRatio(minAspect, maxAspect, blobs);
+             *   A blob's Aspect ratio is the ratio of boxFit long side to short side.
+             *   A perfect Square has an aspect ratio of 1.  All others are > 1
+             */
+            ColorBlobLocatorProcessor.Util.filterByArea(50, 20000, blobs);  // filter out very small blobs.
+
+            /*
+             * The list of Blobs can be sorted using the same Blob attributes as listed above.
+             * No more than one sort call should be made.  Sorting can use ascending or descending order.
+             *     ColorBlobLocatorProcessor.Util.sortByArea(SortOrder.DESCENDING, blobs);      // Default
+             *     ColorBlobLocatorProcessor.Util.sortByDensity(SortOrder.DESCENDING, blobs);
+             *     ColorBlobLocatorProcessor.Util.sortByAspectRatio(SortOrder.DESCENDING, blobs);
+             */
+
+            telemetry.addLine(" Area Density Aspect  Center");
+
+            // Display the size (area) and center location for each Blob.
+            for(ColorBlobLocatorProcessor.Blob b : blobs)
+            {
+                RotatedRect boxFit = b.getBoxFit();
+                telemetry.addLine(String.format("%5d  %4.2f   %5.2f  (%3d,%3d)",
+                          b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) boxFit.center.x, (int) boxFit.center.y));
+            }
+
+            telemetry.update();
+            sleep(50);
+        }
+    }
+}

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

@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2024 Phil Malone
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import android.graphics.Color;
+import android.util.Size;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+import org.firstinspires.ftc.vision.opencv.PredominantColorProcessor;
+
+/*
+ * This OpMode illustrates how to use a video source (camera) as a color sensor
+ *
+ * A "color sensor" will typically determine the color of the object that it is pointed at.
+ *
+ * This sample performs the same function, except it uses a video camera to inspect an object or scene.
+ * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view.
+ * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching color will be selected.
+ *
+ * To perform this function, a VisionPortal runs a PredominantColorProcessor process.
+ *   The PredominantColorProcessor process is created first, and then the VisionPortal is built to use this process.
+ *   The PredominantColorProcessor analyses the ROI and splits the colored pixels into several color-clusters.
+ *   The largest of these clusters is then considered to be the "Predominant Color"
+ *   The process then matches the Predominant Color with the closest Swatch and returns that match.
+ *
+ * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest,
+ * The Predominant Color is used to paint the rectangle border, so the user can verify that the color is reasonable.
+ *
+ * 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
+ */
+
+@Disabled
+@TeleOp(name = "Concept: Vision Color-Sensor", group = "Concept")
+public class ConceptVisionColorSensor extends LinearOpMode
+{
+    @Override
+    public void runOpMode()
+    {
+        /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class.
+         *
+         * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect.
+         *     This can be the entire frame, or a sub-region defined using:
+         *     1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system.
+         *     Use one form of the ImageRegion class to define the ROI.
+         *         ImageRegion.entireFrame()
+         *         ImageRegion.asImageCoordinates(50, 50,  150, 150)  100x100 pixel square near the upper left corner
+         *         ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1)  10% width/height square centered on screen
+         *
+         * - Set the list of "acceptable" color swatches (matches).
+         *     Only colors that you assign here will be returned.
+         *     If you know the sensor will be pointing to one of a few specific colors, enter them here.
+         *     Or, if the sensor may be pointed randomly, provide some additional colors that may match the surrounding.
+         *     Possible choices are:
+         *         RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE;
+         *
+         *     Note that in the example shown below, only some of the available colors are included.
+         *     This will force any other colored region into one of these colors.
+         *     eg: Green may be reported as YELLOW, as this may be the "closest" match.
+         */
+        PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder()
+                .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1))
+                .setSwatches(
+                        PredominantColorProcessor.Swatch.RED,
+                        PredominantColorProcessor.Swatch.BLUE,
+                        PredominantColorProcessor.Swatch.YELLOW,
+                        PredominantColorProcessor.Swatch.BLACK,
+                        PredominantColorProcessor.Swatch.WHITE)
+                .build();
+
+        /*
+         * Build a vision portal to run the Color Sensor process.
+         *
+         *  - Add the colorSensor process created above.
+         *  - Set the desired video resolution.
+         *      Since a high resolution will not improve this process, choose a lower resolution that is
+         *      supported by your camera.  This will improve overall performance and reduce latency.
+         *  - Choose your video source.  This may be
+         *      .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))  .....   for a webcam
+         *  or
+         *      .setCamera(BuiltinCameraDirection.BACK)    ... for a Phone Camera
+         */
+        VisionPortal portal = new VisionPortal.Builder()
+                .addProcessor(colorSensor)
+                .setCameraResolution(new Size(320, 240))
+                .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+                .build();
+
+        telemetry.setMsTransmissionInterval(50);  // Speed up telemetry updates, Just use for debugging.
+
+        // WARNING:  To be able to view the stream preview on the Driver Station, this code runs in INIT mode.
+        while (opModeIsActive() || opModeInInit())
+        {
+            telemetry.addData("DS preview on/off", "3 dots, Camera Stream\n");
+
+            // Request the most recent color analysis.
+            // This will return the closest matching colorSwatch and the predominant RGB color.
+            // Note: to take actions based on the detected color, simply use the colorSwatch in a comparison or switch.
+            //  eg:
+            //      if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {... some code  ...}
+            PredominantColorProcessor.Result result = colorSensor.getAnalysis();
+
+            // Display the Color Sensor result.
+            telemetry.addData("Best Match:", result.closestSwatch);
+            telemetry.addLine(String.format("R %3d, G %3d, B %3d", Color.red(result.rgb), Color.green(result.rgb), Color.blue(result.rgb)));
+            telemetry.update();
+
+            sleep(20);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java

@@ -0,0 +1,115 @@
+package org.firstinspires.ftc.teamcode;
+/*
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.SleepAction;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
+import org.firstinspires.ftc.teamcode.configs.RobotConstants;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
+
+@Autonomous(name = "Auto Test Competition", group = "Dev")
+public class BlueBasketAuto extends OpMode {
+    private Follower follower;
+    private int state;
+
+    private HighBasketAutoPath1 path1;
+    private HighBasketAutoPath2 path2;
+    private AutoPark pathPark;
+    private SkyHookSubsystem hook;
+    private CometBotTeleopCompetition comp;
+    private ElapsedTime runtime;
+
+    private LiftActionsSubsystem liftActionsSubsystem;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.75);
+        path1    = new HighBasketAutoPath1();
+        path2    = new HighBasketAutoPath2();
+        pathPark = new AutoPark();
+
+        comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        comp.initCloseClaw();
+        runtime = new ElapsedTime();
+        hook = new SkyHookSubsystem(hardwareMap);
+        state = 0;
+
+    }
+
+
+    @Override
+    public void loop() {
+        switch(state) {
+            case 0:
+                telemetry.addData("case0", "case0");
+
+                path1.moveToPath1(follower);
+                state = 1;
+                runtime.reset();
+            case 1:
+                if (runtime.seconds() > 5) {
+                    telemetry.addData("case1", "case1");
+
+                    new SleepAction(.5);
+                    comp.highBucketDropAuto();
+
+                    state = 2;
+                }
+            case 2:
+                if (runtime.seconds() > 15) {
+                    telemetry.addData("case2", "case2");
+
+                   // new SleepAsction(.5);
+                    //path2.moveToPath1(follower);
+
+                    //For next time, add encoder control to skyhook and extend here
+                    //comp.moveSkyHook();
+
+                    //pathPark.moveToPark(follower);
+
+                    state = 3;
+                }
+            case 3:
+                if (runtime.seconds() > 15) {
+                    telemetry.addData("case3", "case3");
+                    hook.toLevel1Position();
+
+
+                    state = 4;
+                }
+            case 4:
+                if (runtime.seconds() > 15) {
+                    telemetry.addData("case3", "case3");
+                    hook.toLevel1Position();
+
+
+                    state = 4;
+                }
+                //System.out.println("default");
+                //telemetry.addData("default", "default");
+                //telemetry.update();
+        }
+        telemetry.update();
+        follower.update();
+        //follower.telemetryDebug(telemetry);
+    }
+}
+*/

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java

@@ -1,24 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
-
-@TeleOp(name = "CometBot Auto", group = "Development")
-public class CometBotDevAuto extends OpMode {
-
-    public CometBotAutoDevelopment runMode;
-
-    @Override
-    public void init() {
-        this.runMode = new CometBotAutoDevelopment(hardwareMap, telemetry, gamepad1, gamepad2);
-        this.runMode.init();
-    }
-
-    @Override
-    public void loop() {
-        this.runMode.update();
-        telemetry.update();
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java

@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleOpDevelopment;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+@TeleOp(name = "CometBot Drive V2.137", group = "Competition")
+public class CometBotTeleOpCompetition extends OpMode {
+    public DualMotorSliderSubsystem slide;
+    public CometBotTeleOpDevelopment runMode;
+
+    @Override
+    public void init() {
+        runMode = new CometBotTeleOpDevelopment(hardwareMap, gamepad1, gamepad2);
+        slide = new DualMotorSliderSubsystem(hardwareMap);
+        runMode.init();
+    }
+
+    @Override
+    public void loop() {
+        runMode.update();
+
+        telemetry.update();
+    }
+    public void stop(){
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java

@@ -1,64 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.SleepAction;
-import com.acmerobotics.roadrunner.ftc.Actions;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
-import org.firstinspires.ftc.teamcode.subsystem.AutoLine2;
-import org.firstinspires.ftc.teamcode.subsystem.AutoLine3;
-
-@Autonomous(name = "BlueNetAuto", group = "Dev")
-public class NetAuto extends OpMode {
-
-    public Follower follower;
-
-    public AutoLine1 myFirstPath = new AutoLine1();
-    public AutoLine2 mySecondPath = new AutoLine2();
-    public int pathState = 0;
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-        follower.setMaxPower(0.65);
-        myFirstPath.moveToAutoLine1(follower);
-    }
-
-    @Override
-    public void loop() {
-        follower.update();
-        switch(pathState) {
-            case 0:
-                if (!follower.isBusy()) {
-                    pathState = 1;
-                    mySecondPath.moveToAutoLine2(follower);
-                }
-            case 1:
-                if (!follower.isBusy()) {
-                    System.out.println("Finished");
-                }
-        }
-//        switch(pathState) {
-//            case 0:
-//                if (!follower.isBusy()) {
-//                    mySecondPath.moveToAutoLine2(follower);
-//                    pathState = 1;
-//                }
-//            case 1:
-//                if (!follower.isBusy()) {
-//                    pathState = 2;
-//                }
-//            case 2:
-//                // set path 3
-//                // as if busy, if not, set path 4 and so on.
-//                System.out.print("we're at the end");
-//
-//        }
-
-
-
-        follower.telemetryDebug(telemetry);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java

@@ -39,20 +39,23 @@ public class PedroConstants {
 
     // Robot encoders
     // NOTE:  Encoders are plugged into the same ports as motors hence the weird names
-    public static final String RIGHT_ENCODER = "back-right"; //0
+    public static final String RIGHT_ENCODER = "front-left"; //2
     public static final String BACK_ENCODER = "front-right"; //1
-    public static final String LEFT_ENCODER = "front-left"; //2
+    public static final String LEFT_ENCODER = "back-right"; //0
 
     // Robot encoder direction
     public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
-    public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
-    public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
+    public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
+    public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
 
     // Arm config
-    public static final String SLIDE_MOTOR = "SlideMotor";
-    public static final String Claw_Servo = "ClawServo";
-    public static final String Wrist_Servo = "WristServo";
-    public static final String Arm_Servo = "ArmServo";
+    public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
+    public static final String LIFT_SLIDE_RIGHT_MOTOR = "lift-slide-right";
+    public static final String CLAW_NAME = "claw-servo";
+    public static final String WRIST_SERVO = "wrist-servo";
+    public static final String ARM_SERVO = "arm-servo";
+    public static final String THUMB_SERVO = "thumb-servo";
+    public static final String HOOK = "skyhook";
 
     /*
         Pedro's parameters
@@ -62,21 +65,22 @@ public class PedroConstants {
     public static final double ROBOT_WEIGHT_IN_KG = 9;
 
     // Maximum velocity of the robot going forward
-    public static final double ROBOT_SPEED_FORWARD = 51.5;
+    public static final double ROBOT_SPEED_FORWARD = 50.02;
 
     // Maximum velocity of the robot going right
-    public static final double ROBOT_SPEED_LATERAL = 28.7;
+    public static final double ROBOT_SPEED_LATERAL = 36.07;
 
     // Rate of deceleration when power is cut-off when the robot is moving forward
-    public static final double FORWARD_ZERO_POWER_ACCEL = -59.8;
+    public static final double FORWARD_ZERO_POWER_ACCEL = -91.4557;
 
     // Rate of deceleration when power is cut-off when the robot is moving to the right
-    public static final double LATERAL_ZERO_POWER_ACCEL = -99.7;
+    public static final double LATERAL_ZERO_POWER_ACCEL = -98.514;
 
     // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
-    public static final double ZERO_POWER_ACCEL_MULT = 3.5;
+    public static final double ZERO_POWER_ACCEL_MULT = 4.0;
 
     /* Centripetal force correction - increase if robot is correcting into the path
                                     - decrease if robot is correcting away from the path */
     public static final double CENTRIPETAL_SCALING = 0.0004;
 }
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java

@@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 
-@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
+@Autonomous(name = "PreLoadedBlue", group = "Competition")
 public class PreLoadedBlueBasketAuto extends OpMode {
     private Telemetry telemetryA;
 
@@ -21,13 +21,13 @@ public class PreLoadedBlueBasketAuto extends OpMode {
 
     private PathChain path;
 
-    private final Pose startPose = new Pose(7.875, 89.357);
+    private final Pose startPose = new Pose(8, 55);
 
     @Override
     public void init() {
         follower = new Follower(hardwareMap);
 
-        follower.setMaxPower(.45);
+        follower.setMaxPower(.20);
 
         follower.setStartingPose(startPose);
 
@@ -35,79 +35,11 @@ public class PreLoadedBlueBasketAuto extends OpMode {
                 .addPath(
                         // Line 1
                         new BezierLine(
-                                new Point(8.036, 89.196, Point.CARTESIAN),
-                                new Point(10.125, 126.804, Point.CARTESIAN)
+                                new Point(8, 55, Point.CARTESIAN),
+                                new Point(24, 21, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 2
-                        new BezierCurve(
-                                new Point(10.125, 126.804, Point.CARTESIAN),
-                                new Point(37.607, 90.000, Point.CARTESIAN),
-                                new Point(62.357, 119.893, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(62.357, 119.893, Point.CARTESIAN),
-                                new Point(33.750, 112.500, Point.CARTESIAN),
-                                new Point(15.107, 130.661, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 4
-                        new BezierCurve(
-                                new Point(15.107, 130.661, Point.CARTESIAN),
-                                new Point(58.821, 103.018, Point.CARTESIAN),
-                                new Point(59.625, 126.964, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(59.625, 126.964, Point.CARTESIAN),
-                                new Point(15.107, 130.339, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(15.107, 130.339, Point.CARTESIAN),
-                                new Point(59.625, 126.964, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(59.625, 126.964, Point.CARTESIAN),
-                                new Point(57.857, 133.071, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(57.857, 133.071, Point.CARTESIAN),
-                                new Point(18.964, 134.679, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 9
-                        new BezierCurve(
-                                new Point(18.964, 134.679, Point.CARTESIAN),
-                                new Point(84.536, 131.786, Point.CARTESIAN),
-                                new Point(80.036, 96.429, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-180)).build();
         follower.followPath(path);
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java

@@ -0,0 +1,117 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.roadrunner.SleepAction;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+@Autonomous(name = "Specimen Auto", group = "Development")
+
+public class SpecimenAuto extends OpMode {
+    private Follower follower;
+    private int state;
+    private ElapsedTime runtime;
+
+    private DualMotorSliderSubsystem lift;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+    private HangMotorSubsystem hang;
+
+
+
+    @Override
+     public void init(){
+         lift = new DualMotorSliderSubsystem(hardwareMap);
+         claw = new ClawSubsystem(hardwareMap);
+         arm = new ArmSubsystem(hardwareMap);
+         wrist = new WristSubsystem(hardwareMap);
+         hang = new HangMotorSubsystem(hardwareMap);
+         follower = new Follower(hardwareMap);
+         runtime = new ElapsedTime();
+
+         state = 0;
+         lift.init();
+         claw.init();
+         wrist.InitAuto();
+         arm.initAuto();
+
+
+        follower.setMaxPower(.45);
+
+
+     }
+
+    @Override
+    public void loop() {
+        if(runtime != null){
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
+        telemetry.addData("state", state);
+         switch(state) {
+             case 0:
+                 runtime.reset();
+                 wrist.toSpecimenPrep();
+                 arm.toSpecimenPrep();
+                 if(runtime.seconds() > 0.25){state = 1;}
+                 break;
+             case 1:
+                 //line 1
+                 if(runtime.seconds() > 3){state = 2;}
+                 break;
+             case 2:
+                 lift.toSpecimanHangHeight();
+                 if(runtime.seconds() > 3.75){state = 3;}
+                 break;
+             case 3:
+                wrist.toSpecimenHang();
+                 if(runtime.seconds() > 4){state = 4;}
+                 break;
+             case 4:
+                lift.toSpecimanReleaseHeight();
+                 if(runtime.seconds() > 4.5){state = 5;}
+                 break;
+             case 5:
+                 claw.switchState();
+                 if(runtime.seconds() > 4.65){state = 6;}
+                 break;
+             case 6:
+                 lift.toFloorPosition();
+                 if(runtime.seconds() > 4.75){state = 7;}
+                 break;
+             case 7:
+                 arm.toParkPosition();
+                 wrist.toTravelPosition();
+                 //line 2
+                 if(runtime.seconds() > 0.){state = 8;}
+                 break;
+             case 8:
+                 if(runtime.seconds() > 0.25){state = 9;}
+                 break;
+             case 9:
+                 //specimen drop
+                 break;
+             case 10:
+                 //path 6
+                 break;
+             case 11:
+                 //specimen pickup
+                 break;
+             case 12:
+                 //path 7
+                 break;
+             case 13:
+                 //specimen drop
+                 break;
+         }
+         new SleepAction(5);
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java

@@ -0,0 +1,170 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+@Autonomous(name = "Specimen Line Test", group = "Competition")
+public class SpecimenAutoLineTest extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(8, 55);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.35);
+
+        follower.setStartingPose(startPose);
+
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8.000, 55.000, Point.CARTESIAN),
+                                new Point(39.500, 60.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(39.500, 60.000, Point.CARTESIAN),
+                                new Point(37.000, 60.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(37.000, 60.000, Point.CARTESIAN),
+                                new Point(16.000, 12.000, Point.CARTESIAN),
+                                new Point(80.000, 54.000, Point.CARTESIAN),
+                                new Point(58.000, 23.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(58.000, 23.500, Point.CARTESIAN),
+                                new Point(14.000, 23.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(14.000, 23.500, Point.CARTESIAN),
+                                new Point(58.000, 23.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 6
+                        new BezierCurve(
+                                new Point(58.000, 23.500, Point.CARTESIAN),
+                                new Point(64.000, 6.000, Point.CARTESIAN),
+                                new Point(14.000, 12.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 7
+                        new BezierCurve(
+                                new Point(14.000, 12.000, Point.CARTESIAN),
+                                new Point(60.000, 14.000, Point.CARTESIAN),
+                                new Point(60.000, 7.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(60.000, 7.500, Point.CARTESIAN),
+                                new Point(14.000, 7.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 9
+                        new BezierCurve(
+                                new Point(14.000, 7.500, Point.CARTESIAN),
+                                new Point(34.000, 14.250, Point.CARTESIAN),
+                                new Point(19.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 10
+                        new BezierCurve(
+                                new Point(19.000, 24.000, Point.CARTESIAN),
+                                new Point(39.500, 24.000, Point.CARTESIAN),
+                                new Point(19.000, 64.000, Point.CARTESIAN),
+                                new Point(39.500, 64.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 11
+                        new BezierLine(
+                                new Point(39.500, 64.000, Point.CARTESIAN),
+                                new Point(37.000, 64.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 12
+                        new BezierCurve(
+                                new Point(37.000, 64.000, Point.CARTESIAN),
+                                new Point(19.000, 64.000, Point.CARTESIAN),
+                                new Point(37.000, 24.000, Point.CARTESIAN),
+                                new Point(19.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 13
+                        new BezierCurve(
+                                new Point(19.000, 24.000, Point.CARTESIAN),
+                                new Point(39.500, 24.000, Point.CARTESIAN),
+                                new Point(19.000, 68.000, Point.CARTESIAN),
+                                new Point(39.500, 68.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 14
+                        new BezierLine(
+                                new Point(39.500, 68.000, Point.CARTESIAN),
+                                new Point(37.000, 68.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180)).build();
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueNonBasketAuto.java

@@ -1,151 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "BlueNonBasketAuto", group = "Autonomous Pathing Tuning")
-public class BlueNonBasketAuto extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(10.929, 55.446, 0);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.45);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                // Line 1
-                new BezierCurve(
-                        new Point(10.929, 55.446, Point.CARTESIAN),
-                        new Point(42.429, 46.446, Point.CARTESIAN),
-                        new Point(36.321, 38.089, Point.CARTESIAN)
-                )
-        )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(36.321, 38.089, Point.CARTESIAN),
-                                new Point(59.786, 36.643, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(59.786, 36.643, Point.CARTESIAN),
-                                new Point(59.304, 24.750, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(59.304, 24.750, Point.CARTESIAN),
-                                new Point(13.982, 23.946, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(13.982, 23.946, Point.CARTESIAN),
-                                new Point(59.464, 24.429, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(59.464, 24.429, Point.CARTESIAN),
-                                new Point(58.982, 15.268, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(58.982, 15.268, Point.CARTESIAN),
-                                new Point(13.821, 14.464, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(13.821, 14.464, Point.CARTESIAN),
-                                new Point(58.661, 13.500, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(58.661, 13.500, Point.CARTESIAN),
-                                new Point(58.339, 8.679, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 10
-                        new BezierLine(
-                                new Point(58.339, 8.679, Point.CARTESIAN),
-                                new Point(14.625, 8.518, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0)).build();
-        follower.followPath(path, true);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path, true);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java

@@ -1,142 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots;
-
-import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.SequentialAction;
-import com.acmerobotics.roadrunner.SleepAction;
-import com.acmerobotics.roadrunner.ftc.Actions;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.states.FieldStates;
-import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
-
-public class CometBotAutoDevelopment {
-
-    /*
-    Subsystems
-     */
-    private MotorsSubsystem motors;
-
-    /*
-    Controllers
-     */
-    public Gamepad GP1;
-    public Gamepad GP2;
-    public Gamepad currentGP1;
-    public Gamepad previousGP1;
-    public Gamepad currentGP2;
-    public Gamepad previousGP2;
-    private Telemetry telemetry;
-
-    public FieldStates fieldStates;
-    private boolean centricity = false;
-
-    private Follower follower;
-    private HardwareMap hardwareMap;
-
-    public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
-        this.motors = new MotorsSubsystem(hardwareMap, telemetry);
-        this.GP1 = gp1;
-        this.GP2 = gp2;
-        this.hardwareMap = hardwareMap;
-        this.telemetry = telemetry;
-        this.currentGP1 = new Gamepad();
-        this.currentGP2 = new Gamepad();
-        this.previousGP1 = new Gamepad();
-        this.previousGP2 = new Gamepad();
-        this.fieldStates = new FieldStates();
-        this.follower = new Follower(hardwareMap);
-    }
-
-    public class ZeroOutPower implements Action {
-
-        @Override
-        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-            follower = new Follower(hardwareMap);
-            follower.setMaxPower(0);
-            System.out.println("Running ZeroOutPower");
-            return follower.isBusy();
-        }
-    }
-
-    public class ReturnToMaxPower implements Action {
-
-        @Override
-        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-            follower = new Follower(hardwareMap);
-            follower.setMaxPower(MAX_POWER);
-            follower.startTeleopDrive();
-            System.out.println("Running ReturnToMaxPower");
-            return follower.isBusy();
-        }
-    }
-
-    public Action zeroOutPower() {
-        return new ZeroOutPower();
-    }
-
-    public Action returnToMaxPower() {
-        return new ReturnToMaxPower();
-    }
-
-    public void init() {
-        this.motors.init();
-        this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
-        follower.setMaxPower(MAX_POWER);
-        follower.startTeleopDrive();
-    }
-
-    public void update() {
-        this.previousGP1.copy(currentGP1);
-        this.currentGP1.copy(this.GP1);
-        this.previousGP2.copy(currentGP2);
-        this.currentGP2.copy(this.GP2);
-
-        this.toFixMotorBlockingIssueFirstMethod();
-        this.toFixMotorBlockingIssueSecondMethod();
-        this.changeCentricity();
-
-        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, centricity);
-        follower.update();
-        this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
-    }
-
-    public void changeCentricity() {
-        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
-            this.centricity = !centricity;
-            this.follower.breakFollowing();
-            this.follower.startTeleopDrive();
-        }
-    }
-
-    public void toFixMotorBlockingIssueFirstMethod() {
-        if (this.currentGP1.cross && !this.previousGP1.cross) {
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
-            Actions.runBlocking(new SequentialAction(
-                    this.zeroOutPower(),
-                    new SleepAction(3),
-                    this.returnToMaxPower()
-            ));
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
-        }
-    }
-
-    public void toFixMotorBlockingIssueSecondMethod() {
-        if (this.currentGP1.circle && !this.previousGP1.circle) {
-            this.follower.breakFollowing();
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
-            Actions.runBlocking(new SequentialAction(
-                    new SleepAction(3)
-            ));
-            fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
-            this.follower.startTeleopDrive();
-        }
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java

@@ -0,0 +1,215 @@
+package org.firstinspires.ftc.teamcode.cometbots;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+public class CometBotTeleOpDevelopment {
+
+    /*
+    Subsystems
+     */
+    private DualMotorSliderSubsystem dualSlides;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+    private HangMotorSubsystem skyhook;
+    /*
+    Controllers
+     */
+    public Gamepad gamepad1;
+    public Gamepad gamepad2;
+    public Gamepad currentGamepad1;
+    public Gamepad currentGamepad2;
+    public Gamepad previousGamepad1;
+    public Gamepad previousGamepad2;
+
+    public boolean wristPickup;
+    public boolean armParked;
+    public boolean goClaw;
+    private Follower follower;
+
+    public CometBotTeleOpDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
+        dualSlides = new DualMotorSliderSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        skyhook = new HangMotorSubsystem(hardwareMap);
+
+        this.gamepad1 = gamepad1;
+        this.gamepad2 = gamepad2;
+        currentGamepad1 = new Gamepad();
+        currentGamepad2 = new Gamepad();
+        previousGamepad1 = new Gamepad();
+        previousGamepad2 = new Gamepad();
+        follower = new Follower(hardwareMap);
+
+    }
+
+    public void init() {
+        dualSlides.init();
+        claw.init();
+        wrist.initTeleOp();
+        arm.initTeleOp();
+        skyhook.init();
+        follower.setMaxPower(MAX_POWER);
+        follower.startTeleopDrive();
+        wristPickup = false;
+        armParked = true;
+        goClaw = false;
+    }
+
+    public void update() {
+        previousGamepad1.copy(currentGamepad1);
+        currentGamepad1.copy(gamepad1);
+        previousGamepad2.copy(currentGamepad2);
+        currentGamepad2.copy(gamepad2);
+
+        //slides
+        dualSlides.update();
+        dualSlidesToLowBucketPosition();
+        dualSlidesToHighBucketPosition();
+        dualSlidesToFloorPosition();
+        //arm
+        armToParkPosition();
+        armAndWristToFloor();
+        armToBucketPosition();
+        //claw
+        openClaw();
+        openThumb();
+        //hang
+        hang();
+        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
+        follower.update();
+    }
+    private void openClaw() {
+        if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
+            claw.switchState();
+        }
+    }
+
+    private void openThumb() {
+        if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) {
+            claw.switchTState();
+
+        }
+    }
+
+    private void armAndWristToFloor() {
+        if (currentGamepad2.a && !previousGamepad2.a && wristPickup) {
+            if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
+                wrist.toFloorPositionTeleop();
+            } else if (wristPickup) {
+                claw.openClaw();
+                wrist.toPickupPosition();
+            }
+        }
+        if (currentGamepad2.a && !previousGamepad2.a) {
+            arm.toFloorPositionTeleOp();
+
+            wristPickup = true;
+            armParked = false;
+        }
+    }
+
+    private void armToBucketPosition() {
+        if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) {
+            armParked = false;
+            arm.toBucketPosition();
+            wrist.toBucketPosition();
+            wristPickup = false;
+
+        }
+    }
+
+    private void armToParkPosition() {
+        if (currentGamepad2.x && !previousGamepad2.x) {
+            armParked = true;
+            arm.toParkPosition();
+            wrist.toFloorPositionTeleop();
+            wristPickup = false;
+
+        }
+    }
+
+    private void dualSlidesToHighBucketPosition() {
+        if (currentGamepad2.y && !previousGamepad2.y && armParked) {
+            dualSlides.toHighBucketPosition();
+        }
+    }
+
+    private void dualSlidesToFloorPosition() {
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down && armParked) {
+            dualSlides.toFloorPosition();
+        }
+    }
+
+    private void dualSlidesToLowBucketPosition() {
+        if (currentGamepad2.b && !previousGamepad2.b && armParked) {
+            dualSlides.toLowBucketPosition();
+        }
+    }
+    private void hang(){
+        if (gamepad1.a) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
+            arm.grabBlueberrySkyhook();
+
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+        }
+        if(gamepad1.b) {
+            //confirm grab
+            claw.closeClaw();
+
+        }
+
+        if (gamepad1.x && claw.getState() == ClawSubsystem.ClawState.CLOSED) {
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+            dualSlides.toFixedPosition(200);
+            dualSlides.update();
+        }
+
+        if (gamepad1.y) {
+            wrist.hangBlueberrySkyhook();
+            arm.hangBlueberrySkyhook();
+            try {
+                Thread.sleep(1500);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            goClaw = true;
+            dualSlides.toFixedPosition(2100);
+            dualSlides.update();
+        }
+        if (gamepad1.right_bumper) {
+            claw.openClaw();
+            if(goClaw == true) {
+                try {
+                    Thread.sleep(500);
+                } catch (InterruptedException e) {
+                    throw new RuntimeException(e);
+                }
+                dualSlides.toFloorPosition();
+                dualSlides.update();
+                arm.toParkPosition();
+                wrist.toPickupPosition();
+            }
+        }
+        skyhook.setPowerForward(-gamepad2.right_stick_y);
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java

@@ -0,0 +1,331 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+@Autonomous(name = "Auto Competition V2", group = "A")
+public class CometBotDriveV2 extends OpMode {
+    private Follower follower;
+    private int state;
+
+    private HighBasketPath1 path1;
+    private HighBasketPath2 path2;
+    private HighBasketPath3 path3;
+    private HighBasketPath4 path4;
+    private HighBasketPath5 path5;
+    private HighBasketPath6 path6;
+
+
+    //private CometBotTeleopCompetition comp;
+    private static ElapsedTime runtime;
+    private static boolean initalized = false;
+    private static boolean followingPath = false;
+    private DualMotorSliderSubsystem lift;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.75);
+
+        path1 = new HighBasketPath1();
+        path2 = new HighBasketPath2();
+        path3 = new HighBasketPath3();
+        path4 = new HighBasketPath4();
+        path5 = new HighBasketPath5();
+        path6 = new HighBasketPath6();
+        lift = new DualMotorSliderSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+
+
+        lift.init();
+        arm.initAuto();
+        wrist.InitAuto();
+        claw.init();
+
+        //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        // comp.initCloseClaw();
+        runtime = new ElapsedTime();
+    }
+
+
+    public void loop() {
+        telemetry.addData("state", state);
+//        telemetry.addData("arm", arm);
+//        telemetry.addData("lift", lift);
+//        telemetry.addData("wrist", wrist);
+//        telemetry.addData("claw", claw);
+        telemetry.addData("followingPath", followingPath);
+        telemetry.addData("busy", follower.isBusy());
+
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
+        switch (state) {
+            case 0:
+
+                runtime.reset();
+                moveToPathOneAndHighBucket();
+                break;
+            case 1:
+                doArmThing();
+                state = 2;
+                break;
+            case 2:
+                moveToPathTwoAndPickSampleUp();
+                break;
+            case 3:
+                doPickUpThing();
+                break;
+            case 4:
+                moveToBasketPath3();
+                break;
+            case 5:
+                theArmThing();
+                break;
+            case 6:
+                moveToPickupAgainPath4();
+                break;
+            case 7:
+                doPickUpThingAgain();
+                break;
+            case 8:
+                moveToBasketPath5();
+                break;
+            case 9:
+                theArmThingAgain();
+                break;
+            case 10:
+                moveToParkPath6();
+                break;
+            case 11:
+                moveToPark();
+                break;
+            case 99:
+                wrist.toFloorPosition();
+                break;
+            default:
+                telemetry.addLine("default");
+
+        }
+        telemetry.update();
+        lift.update();
+        follower.update();
+
+
+    }
+
+    public void moveToPark() {
+        if (runtime.seconds() > 26) {
+            path6.moveToBasketPath6(follower);
+            state = 99;
+        }
+    }
+
+    private void moveToPathOneAndHighBucket() {
+        path1.moveToPath1(follower);
+        state = 1;
+    }
+    private void moveToBasketPath5() {
+        if(runtime.seconds() > 19){
+            path5.moveToBasketPath5(follower);
+            lift.toHighBucketReverseDrop();
+            wrist.toTravelPosition();
+            state = 9;
+        }
+
+    }
+
+
+    public class SetStateAction implements Action {
+
+        private int value;
+
+        public SetStateAction(int value) {
+            this.value = value;
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+            state = value;
+            return false;
+        }
+    }
+
+    private Action setStateValue(int value) {
+        return new SetStateAction(value);
+    }
+
+    private void doArmThing() {
+
+        claw.thumbOutOfTheWay();
+        wrist.toTravelPosition();
+        lift.toHighBucketReverseDrop();
+        //arm.toReverseBucket();
+        //wrist.toReverseBucket();
+        //claw.openClaw();
+        //wrist.toFloorPosition();
+
+    }
+
+
+    private void theArmThing() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 12) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 12.75) {
+                claw.autoOpenClaw();
+                state = 6;
+            }
+        }
+    }
+
+    private void theArmThingAgain() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 22) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 23.00) {
+                claw.autoOpenClaw();
+                state = 10;
+            }
+        }
+    }
+
+
+    private void moveToPathTwoAndPickSampleUp() {
+        if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
+            wrist.toReverseBucket();
+        }
+        if (runtime.seconds() > 3.5) {
+            claw.autoOpenClaw();
+        }
+        if (runtime.seconds() > 4) {
+            wrist.toTravelPosition();
+            lift.toFloorPosition();
+            path2.moveToPath2(follower);
+            state = 3;
+        }
+    }
+
+    //
+    private void moveToBasketPath3() {
+        if (runtime.seconds() > 7.25) {
+            lift.toHighBucketReverseDrop();
+            path3.moveToBasketPath3(follower);
+            state = 5;
+        }
+    }
+
+    private void moveToPickupAgainPath4() {
+        if (runtime.seconds() > 13) {
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 14) {
+            lift.toFloorPosition();
+            path4.moveToPickupPath4(follower);
+            state = 7;
+        }
+    }
+
+
+
+    private void moveToParkPath6() {
+        if (runtime.seconds() > 24.5) {
+            arm.toBucketPosition();
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 25.) {
+            lift.toFloorPosition();
+            claw.closeClaw();
+            state = 11;
+        }
+    }
+//
+//    private void moveToBasketPath3() {
+//        if (!followingPath) {
+//            path3.moveToBasketPath3(follower);
+//            followingPath = true;
+//        }
+//        if (runtime != null) {
+//            telemetry.addData("Runtime (seconds)", runtime.seconds());
+//            if (follower.atParametricEnd() || runtime.seconds() > 10.0) {
+//                state = 5;
+//                followingPath = false;
+//            }
+//        }
+//    }
+//
+//    private void thePickUpAuto() {
+//        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.1),
+//                comp.claw.openClaw(),
+//                new SleepAction(.2),
+//                comp.wrist.toPickupPosition(),
+//                new SleepAction(.2),
+//                comp.arm.toSubmarinePosition(),
+//                new SleepAction(.5),
+//                comp.claw.closeClaw(),
+//                new SleepAction(.3),
+//                comp.wrist.toFloorPosition(),
+//                new SleepAction(.2),
+//                comp.arm.toParkPosition(),
+//                new SleepAction(.2)
+//        ));
+//    }
+//    private void thePickUp() {
+//        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.5),
+//                comp.wrist.toPickupPosition(),
+//                new SleepAction(.5),
+//                comp.arm.toSubmarinePosition(),
+//                new SleepAction(.7),
+//                comp.claw.closeClaw(),
+//                new SleepAction(.7),
+//                comp.wrist.toFloorPosition(),
+//                new SleepAction(.5),
+//                comp.arm.toParkPosition(),
+//                new SleepAction(.5)
+//        ));
+//    }
+//
+    private void doPickUpThing() {
+        wrist.toPickupPosition();
+        arm.toFloorPosition();
+        if (runtime.seconds() > 7){
+            claw.closeClaw();
+            state = 4;
+        }
+    }
+
+    private void doPickUpThingAgain() {
+       wrist.toPickupPosition();
+       arm.toFloorPosition();
+        if (runtime.seconds() > 18){
+            claw.closeClaw();
+            state = 8;
+        }
+    }
+
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java

@@ -0,0 +1,366 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
+
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.states.FieldStates;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+public class CometBotTeleopCompetition {
+
+    /*
+    Subsystems
+     */
+    private MotorsSubsystem motors;
+    private ClawSubsystem claw;
+    private ArmSubsystem arm;
+    private WristSubsystem wrist;
+    private DualMotorSliderSubsystem lift;
+
+    /*
+    Controllers
+     */
+    public Gamepad GP1;
+    public Gamepad GP2;
+    public Gamepad currentGP1;
+    public Gamepad previousGP1;
+    public Gamepad currentGP2;
+    public Gamepad previousGP2;
+
+    /*
+    Pedro/FTC Components
+     */
+    private Follower follower;
+    private Telemetry telemetry;
+
+    /*
+    States
+     */
+    public FieldStates fieldStates;
+
+    /*
+    Configurations
+     */
+    public double currentPower = MAX_POWER;
+
+    /*
+    Misc
+     */
+    private int hbCounter = 0;
+
+    public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
+        this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
+
+        this.GP1 = gp1;
+        this.GP2 = gp2;
+        this.telemetry = telemetry;
+        this.currentGP1 = new Gamepad();
+        this.currentGP2 = new Gamepad();
+        this.previousGP1 = new Gamepad();
+        this.previousGP2 = new Gamepad();
+        this.fieldStates = new FieldStates();
+        this.follower = new Follower(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        lift = new DualMotorSliderSubsystem(hardwareMap);
+    }
+
+    public void init() {
+        this.motors.init();
+        claw.init();
+        arm.initTeleOp();
+        lift.init();
+       ;
+        this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+
+        follower.setMaxPower(MAX_POWER);
+        follower.startTeleopDrive();
+        claw.closeClaw();
+        wrist.initTeleOp();
+    }
+
+    public void initCloseClaw(){
+        this.motors.init();
+        this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+        follower.setMaxPower(MAX_POWER);
+        follower.startTeleopDrive();
+    }
+
+    public void update() {
+        this.previousGP1.copy(currentGP1);
+        this.currentGP1.copy(this.GP1);
+        this.previousGP2.copy(currentGP2);
+        this.currentGP2.copy(this.GP2);
+//        this.moveSkyHook();
+        this.toHighBucketScore();
+        this.toLowBucketScore();
+        this.toArmParkPosition();
+        this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
+        this.clawControl();
+//        this.decreaseMaxPower();
+//        this.increaseMaxPower();
+
+
+//        Actions.runBlocking(this.lift.toFloorPosition());
+
+        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x);
+        follower.update();
+        lift.update();
+        this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
+//        this.telemetry.addData("Claw State", this.claw.getState());
+//        this.telemetry.addData("Claw Position", this.claw.getPosition());
+//        this.telemetry.addData("Wrist State", this.wrist.getState());
+//        this.telemetry.addData("Arm State", this.arm.getState());
+//        this.telemetry.addData("Lift State", this.lift.getState());
+//        this.telemetry.addData("Lift Position", this.lift.getPosition());
+        this.telemetry.addData("MaxPower", MAX_POWER);
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Decreases maximum speed by -.05
+     */
+//    public void decreaseMaxPower() {
+//        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
+//            this.currentPower = this.currentPower - .05;
+//            this.follower.setMaxPower(this.currentPower);
+//        }
+//    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Increases maximum speed by +.05
+     */
+//    public void increaseMaxPower() {
+//        if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
+//            this.currentPower = this.currentPower + .05;
+//            this.follower.setMaxPower(this.currentPower);
+//        }
+//    }
+//    public void moveSkyHook() {
+//        if (this.currentGP2.dpad_down) {
+//            hook.moveSkyHook(-1.00);
+//        }
+//        else if (this.currentGP2.dpad_up) {
+//            hook.moveSkyHook(1.00);
+//        }
+//        else{
+//            hook.moveSkyHook(0.00);
+//        }
+//
+//    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: TRIANGLE / Y
+    Assumption: Claw is holding specimen, robot is facing buckets ready to score
+    Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
+            raises to high bucket.  Once at high bucket position, move arm forward, wrist forward
+            and open claw to drop specimen into bucket.  Finally, put arm back up and wrist back up,
+            retract lift all the way down to floor position and back to TRAVELING state.
+     */
+    public void toHighBucketScore() {
+        if (this.currentGP2.y && !this.previousGP2.y) {
+            switch(hbCounter) {
+                case 0:
+                    lift.toHighBucketPosition();
+                    hbCounter = 1;
+                    break;
+                case 1:
+                    wrist.toBucketPosition();
+                    arm.toBucketPosition();
+                    hbCounter = 2;
+                    break;
+                case 2:
+                    claw.openClaw();
+                    hbCounter = 3;
+                    break;
+                case 3:
+                    claw.closeClaw();
+                    wrist.toTravelPosition();
+                    arm.toParkPosition();
+                    hbCounter = 4;
+                    break;
+                case 4:
+                    lift.toFloorPosition();
+                    break;
+                default:
+                    break;
+            }
+            hbCounter = 1;
+            if (lift.getFixedPosition() >= 4450) {
+                arm.toBucketPosition();
+                if(arm.getPosition() == 0.45);
+                    wrist.toBucketPosition();
+                    if(wrist.getPosition() == 0.56);
+                        claw.openClaw();
+            }
+        }
+    }
+
+    public void highBucketDrop() {
+        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.5),
+//                this.lift.toHighBucketPosition(),
+//                new SleepAction(.5),
+//                this.arm.toBucketPosition(),
+//                new SleepAction(.5),
+//                this.wrist.toBucketPosition(),
+//                new SleepAction(.5),
+//                this.claw.openClaw(),
+//                new SleepAction(.5),
+//                this.wrist.toFloorPosition(),
+//                new SleepAction(.5),
+//                this.arm.toParkPosition(),
+//                this.lift.toFloorPosition(),
+//                new SleepAction(.5)
+        ));
+    }
+    public void highBucketDropAuto() {
+        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.1),
+//                this.lift.toHighBucketPosition(),
+//                new SleepAction(.25),
+//                this.arm.toBucketPosition(),
+//                new SleepAction(.25),
+//                this.wrist.toBucketPosition(),
+//                new SleepAction(.25),
+//                this.claw.openClaw(),
+//                new SleepAction(.25),
+//                this.wrist.toFloorPosition(),
+//                new SleepAction(.25),
+//                this.arm.toParkPosition(),
+//                this.lift.toZeroPosition(),
+//                new SleepAction(.25)
+        ));
+    }
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: CIRCLE / B
+    Assumption: Claw is holding specimen, robot is facing buckets ready to score
+    Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
+            raises to low bucket.  Once at low bucket position, move arm forward, wrist forward
+            and open claw to drop specimen into bucket.  Finally, put arm back up and wrist back up,
+            retract lift all the way down to floor position and back to TRAVELING state.
+     */
+    public void toLowBucketScore() {
+        if (this.currentGP2.b && !this.previousGP2.b) {
+        lift.toLowBucketPosition();
+            if (lift.getFixedPosition() >= 1700); {
+                arm.toBucketPosition();
+                if(arm.getPosition() == 0.45)
+                    wrist.toBucketPosition();
+                if(wrist.getPosition() == 0.56)
+                    claw.openClaw();
+            }
+        }
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: RIGHT BUMPER
+    Assumption: Working claw mechanism
+    Action: On button press, claw switches state from OPEN to CLOSE
+     */
+    public void clawControl() {
+        if (this.currentGP2.right_bumper && !this.previousGP2.right_bumper) {
+            claw.switchState();
+        }
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: SQUARE / X
+    Assumption: Working arm mechanism
+    Action: On button press, pulls arm up and wrist up, ideal for traveling the field when
+            holding a specimen in claws
+     */
+    public void toArmParkPosition() {
+        if (this.currentGP2.square && !this.previousGP2.square) {
+            Actions.runBlocking(new SequentialAction(
+//                    this.wrist.toFloorPosition(),
+//                    this.arm.toParkPosition(),
+//                    this.lift.toFloorPosition()
+            ));
+        }
+    }
+
+
+
+
+    /*
+    Type: PS4
+    Controller: 2
+    Button: CROSS / A
+    Assumption: Working claw, arm and wrist mechanisms
+    Action: On button press, if arm is in PARK (toArmParkPosition), drop the arm to SUBMARINE
+            position.  SUBMARINE position means the arm and wrist are parallel to the floor, raised
+            3 INCHES off the ground.  This state is ideal for moving the arm into the SUBMARINE
+            area of the field.
+
+            When arm is in SUBMARINE position, pressing the button again puts the arm and wrist into
+            FLOOR state.  This angles the arm and wrist down so that it is able to pick specimens
+            from within the SUBMARINE floor.
+     */
+    public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() {
+        if (this.currentGP2.cross && !previousGP2.cross) {
+//            if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
+//                Actions.runBlocking(
+//                        new SequentialAction(
+//                                this.arm.toSubmarinePosition(),
+//                                this.wrist.toFloorPosition()
+//                        )
+//                );
+//            } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) {
+//                Actions.runBlocking(
+//                        new SequentialAction(
+////                                this.lift.toFloorPosition(),
+//                                this.arm.toSubmarinePosition(),
+//                                this.wrist.toPickupPosition()
+//                        )
+//                );
+//            } else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) {
+//                Actions.runBlocking(
+//                        new SequentialAction(
+//                                //  this.arm.toSubmarinePosition(),
+//                                this.wrist.toFloorPosition()
+//                        )
+//                );
+//            }
+        }
+    }
+
+
+
+
+
+
+
+
+
+
+}
+
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java

@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+
+@TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition")
+@Disabled
+public class CometBotTeleopDriveV2 extends OpMode {
+
+    public CometBotTeleopCompetition runMode;
+
+    @Override
+    public void init() {
+        this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        this.runMode.init();
+
+    }
+
+    @Override
+    public void loop() {
+        this.runMode.update();
+        telemetry.update();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CopyofCometBotDriveV2.java

@@ -0,0 +1,333 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+@Autonomous(name = "Copy of Auto Competition V2", group = "A")
+public class CopyofCometBotDriveV2 extends OpMode {
+    private Follower follower;
+    private int state;
+
+    private HighBasketPath1 path1;
+    private HighBasketPath2 path2;
+    private HighBasketPath3 path3;
+    private HighBasketPath4 path4;
+    private HighBasketPath5 path5;
+    //private HighBasketPath6 path6;
+    private HighBasketPath6b path6;
+
+
+    //private CometBotTeleopCompetition comp;
+    private static ElapsedTime runtime;
+    private static boolean initalized = false;
+    private static boolean followingPath = false;
+    private DualMotorSliderSubsystem lift;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(.75);
+
+        path1 = new HighBasketPath1();
+        path2 = new HighBasketPath2();
+        path3 = new HighBasketPath3();
+        path4 = new HighBasketPath4();
+        path5 = new HighBasketPath5();
+        path6 = new HighBasketPath6b();
+        //path6 = new HighBasketPath6();
+        lift = new DualMotorSliderSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+
+
+        lift.init();
+        arm.initAuto();
+        wrist.InitAuto();
+        claw.init();
+
+        //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        // comp.initCloseClaw();
+        runtime = new ElapsedTime();
+    }
+
+
+    public void loop() {
+        telemetry.addData("state", state);
+//        telemetry.addData("arm", arm);
+//        telemetry.addData("lift", lift);
+//        telemetry.addData("wrist", wrist);
+//        telemetry.addData("claw", claw);
+        telemetry.addData("followingPath", followingPath);
+        telemetry.addData("busy", follower.isBusy());
+
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
+        switch (state) {
+            case 0:
+
+                runtime.reset();
+                moveToPathOneAndHighBucket();
+                break;
+            case 1:
+                doArmThing();
+                state = 2;
+                break;
+            case 2:
+                moveToPathTwoAndPickSampleUp();
+                break;
+            case 3:
+                doPickUpThing();
+                break;
+            case 4:
+                moveToBasketPath3();
+                break;
+            case 5:
+                theArmThing();
+                break;
+            case 6:
+                moveToPickupAgainPath4();
+                break;
+            case 7:
+                doPickUpThingAgain();
+                break;
+            case 8:
+                moveToBasketPath5();
+                break;
+            case 9:
+                theArmThingAgain();
+                break;
+            case 10:
+                moveToParkPath6();
+                break;
+            case 11:
+                moveToPark();
+                break;
+            case 99:
+                wrist.toFloorPosition();
+                break;
+            default:
+                telemetry.addLine("default");
+
+        }
+        telemetry.update();
+        lift.update();
+        follower.update();
+
+
+    }
+
+    public void moveToPark() {
+        if (runtime.seconds() > 26) {
+            path6.moveToBasketPath6(follower);
+            state = 99;
+        }
+    }
+
+    private void moveToPathOneAndHighBucket() {
+        path1.moveToPath1(follower);
+        state = 1;
+    }
+    private void moveToBasketPath5() {
+        if(runtime.seconds() > 19){
+            path5.moveToBasketPath5(follower);
+            lift.toHighBucketReverseDrop();
+            wrist.toTravelPosition();
+            state = 9;
+        }
+
+    }
+
+
+    public class SetStateAction implements Action {
+
+        private int value;
+
+        public SetStateAction(int value) {
+            this.value = value;
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+            state = value;
+            return false;
+        }
+    }
+
+    private Action setStateValue(int value) {
+        return new SetStateAction(value);
+    }
+
+    private void doArmThing() {
+
+        claw.thumbOutOfTheWay();
+        wrist.toTravelPosition();
+        lift.toHighBucketReverseDrop();
+        //arm.toReverseBucket();
+        //wrist.toReverseBucket();
+        //claw.openClaw();
+        //wrist.toFloorPosition();
+
+    }
+
+
+    private void theArmThing() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 12) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 12.75) {
+                claw.autoOpenClaw();
+                state = 6;
+            }
+        }
+    }
+
+    private void theArmThingAgain() {
+        if(lift.getFixedPosition() >= 4280){
+            arm.toReverseBucket();
+            if(runtime.seconds() > 22) {
+                wrist.toReverseBucket();
+            }
+            if(runtime.seconds() > 23.00) {
+                claw.autoOpenClaw();
+                state = 10;
+            }
+        }
+    }
+
+
+    private void moveToPathTwoAndPickSampleUp() {
+        if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
+            wrist.toReverseBucket();
+        }
+        if (runtime.seconds() > 3.5) {
+            claw.autoOpenClaw();
+        }
+        if (runtime.seconds() > 4) {
+            wrist.toTravelPosition();
+            lift.toFloorPosition();
+            path2.moveToPath2(follower);
+            state = 3;
+        }
+    }
+
+    //
+    private void moveToBasketPath3() {
+        if (runtime.seconds() > 7.25) {
+            lift.toHighBucketReverseDrop();
+            path3.moveToBasketPath3(follower);
+            state = 5;
+        }
+    }
+
+    private void moveToPickupAgainPath4() {
+        if (runtime.seconds() > 13) {
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 14) {
+            lift.toFloorPosition();
+            path4.moveToPickupPath4(follower);
+            state = 7;
+        }
+    }
+
+
+
+    private void moveToParkPath6() {
+        if (runtime.seconds() > 24.5) {
+            arm.toBucketPosition();
+            wrist.toTravelPosition();
+        }
+        if (runtime.seconds() > 25.) {
+            lift.toFloorPosition();
+            claw.closeClaw();
+            state = 11;
+        }
+    }
+//
+//    private void moveToBasketPath3() {
+//        if (!followingPath) {
+//            path3.moveToBasketPath3(follower);
+//            followingPath = true;
+//        }
+//        if (runtime != null) {
+//            telemetry.addData("Runtime (seconds)", runtime.seconds());
+//            if (follower.atParametricEnd() || runtime.seconds() > 10.0) {
+//                state = 5;
+//                followingPath = false;
+//            }
+//        }
+//    }
+//
+//    private void thePickUpAuto() {
+//        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.1),
+//                comp.claw.openClaw(),
+//                new SleepAction(.2),
+//                comp.wrist.toPickupPosition(),
+//                new SleepAction(.2),
+//                comp.arm.toSubmarinePosition(),
+//                new SleepAction(.5),
+//                comp.claw.closeClaw(),
+//                new SleepAction(.3),
+//                comp.wrist.toFloorPosition(),
+//                new SleepAction(.2),
+//                comp.arm.toParkPosition(),
+//                new SleepAction(.2)
+//        ));
+//    }
+//    private void thePickUp() {
+//        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.5),
+//                comp.wrist.toPickupPosition(),
+//                new SleepAction(.5),
+//                comp.arm.toSubmarinePosition(),
+//                new SleepAction(.7),
+//                comp.claw.closeClaw(),
+//                new SleepAction(.7),
+//                comp.wrist.toFloorPosition(),
+//                new SleepAction(.5),
+//                comp.arm.toParkPosition(),
+//                new SleepAction(.5)
+//        ));
+//    }
+//
+    private void doPickUpThing() {
+        wrist.toPickupPosition();
+        arm.toFloorPosition();
+        if (runtime.seconds() > 7){
+            claw.closeClaw();
+            state = 4;
+        }
+    }
+
+    private void doPickUpThingAgain() {
+       wrist.toPickupPosition();
+       arm.toFloorPosition();
+        if (runtime.seconds() > 18){
+            claw.closeClaw();
+            state = 8;
+        }
+    }
+
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java

@@ -1,9 +1,5 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.cometbots.paths;
 
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
@@ -14,29 +10,28 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 
 
-
 /*
     AutoLine# - This file does something of a path......
 
  */
-public class AutoLine1 {
+public class HighBasketPath1 {
 
-    private PathChain pathChain;
-    private Pose autoLin1StartPose = new Pose(8,65);
+    private final Pose startPose = new Pose(10, 89);
 
-    public void moveToAutoLine1(Follower robot) {
+    public void moveToPath1(Follower robot) {
+        PathChain pathChain;
+        robot.setStartingPose(startPose);
         PathBuilder builder = new PathBuilder();
         builder
                 .addPath(
                         // Line 1
                         new BezierLine(
-                                new Point(8.000, 65.000, Point.CARTESIAN),
-                                new Point(30.000, 72.000, Point.CARTESIAN)
+                                new Point(10.000, 89.000, Point.CARTESIAN),
+                                new Point(20.000, 130.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0));
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
         pathChain = builder.build();
-        robot.setStartingPose(autoLin1StartPose);
         robot.followPath(pathChain);
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath2 {
+
+
+    public void moveToPath2(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(20.000, 130.000, Point.CARTESIAN),
+                                new Point(27.000, 117.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath3 {
+
+
+    public void moveToBasketPath3(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(26.000, 117.000, Point.CARTESIAN),
+                                new Point(22.000, 132.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath4 {
+
+
+    public void moveToPickupPath4(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(22.000, 132.000, Point.CARTESIAN),
+                                new Point(27.000, 128.500, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath5.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath5 {
+
+
+    public void moveToBasketPath5(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(27.000, 127.000, Point.CARTESIAN),
+                                new Point(22.000, 132.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java

@@ -1,12 +1,7 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.cometbots.paths;
 
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
@@ -18,23 +13,25 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
     AutoLine# - This file does something of a path......
 
  */
-public class AutoLine2 {
-    private PathChain pathChain;
-    public void moveToAutoLine2(Follower robot) {
+public class HighBasketPath6 {
+
+
+    public void moveToBasketPath6(Follower robot) {
+        PathChain pathChain;
         PathBuilder builder = new PathBuilder();
         builder
                 .addPath(
-                        // Line 2
+                        // Line 1
                         new BezierCurve(
-                                new Point(36.000, 72.000, Point.CARTESIAN),
-                                new Point(24.000, 24.000, Point.CARTESIAN),
-                                new Point(72.000, 36.000, Point.CARTESIAN),
-                                new Point(56.000, 24.000, Point.CARTESIAN)
+                                new Point(27.000, 128.000, Point.CARTESIAN),
+                                new Point(80.000, 130.000, Point.CARTESIAN),
+                                new Point(100.000, 118.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180));
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270));
         pathChain = builder.build();
         robot.followPath(pathChain);
     }
+
 }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6b.java

@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath6b {
+
+
+    public void moveToBasketPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(27.000, 128.000, Point.CARTESIAN),
+                                new Point(92.448, 125.671, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(270))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(92.448, 125.671, Point.CARTESIAN),
+                                new Point(93.000, 94.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation();
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java

@@ -1,135 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning")
-public class AsherPathV1 extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(10.0, 40, 90);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.4);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                /*
-                * Only update this path
-                 */
-                .addPath(
-                        // Line 1
-                        new BezierCurve(
-                                new Point(9.757, 84.983, Point.CARTESIAN),
-                                new Point(33.000, 105.000, Point.CARTESIAN),
-                                new Point(80.000, 118.000, Point.CARTESIAN),
-                                new Point(55.000, 120.000, Point.CARTESIAN)
-                        )
-                )
-                .addPath(
-                        // Line 2
-                        new BezierCurve(
-                                new Point(55.000, 120.000, Point.CARTESIAN),
-                                new Point(22.000, 106.000, Point.CARTESIAN),
-                                new Point(11.000, 131.000, Point.CARTESIAN)
-                        )
-                )
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(11.000, 131.000, Point.CARTESIAN),
-                                new Point(75.000, 95.000, Point.CARTESIAN),
-                                new Point(112.000, 132.000, Point.CARTESIAN),
-                                new Point(61.000, 131.000, Point.CARTESIAN)
-                        )
-                )
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(61.000, 131.000, Point.CARTESIAN),
-                                new Point(11.000, 131.000, Point.CARTESIAN)
-                        )
-                )
-                .addPath(
-                        // Line 5
-                        new BezierCurve(
-                                new Point(11.000, 131.000, Point.CARTESIAN),
-                                new Point(100.000, 118.000, Point.CARTESIAN),
-                                new Point(103.000, 135.000, Point.CARTESIAN),
-                                new Point(61.000, 135.000, Point.CARTESIAN)
-                        )
-                )
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(61.000, 135.000, Point.CARTESIAN),
-                                new Point(11.000, 131.000, Point.CARTESIAN)
-                        )
-                )
-                .addPath(
-                        // Line 7
-                        new BezierCurve(
-                                new Point(11.000, 131.000, Point.CARTESIAN),
-                                new Point(113.000, 95.000, Point.CARTESIAN),
-                                new Point(67.000, 95.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
-                /*
-                * End of only update this path
-                 */
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExample.java

@@ -1,79 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning")
-public class AutoExample extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(0.0, 20.0, 0);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.6);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(0.000, 20.000, Point.CARTESIAN),
-                                new Point(50.000, 20.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleFour.java

@@ -1,106 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning")
-public class AutoExampleFour extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(12,60, 0);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.45);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierCurve(
-                                new Point(12.000, 60.000, Point.CARTESIAN),
-                                new Point(60.000, 60.000, Point.CARTESIAN),
-                                new Point(60.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(60.000, 12.000, Point.CARTESIAN),
-                                new Point(40.000, 12.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
-                .addPath(
-                        // Line 3
-                        new BezierCurve(
-                                new Point(40.000, 12.000, Point.CARTESIAN),
-                                new Point(35.000, 35.000, Point.CARTESIAN),
-                                new Point(12.000, 35.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(12.000, 35.000, Point.CARTESIAN),
-                                new Point(12.000, 60.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(0))
-                .build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleSeason2025V1.java

@@ -1,142 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
-public class AutoExampleSeason2025V1 extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(15.0, 35, 90);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.375);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(15.000, 35.000, Point.CARTESIAN),
-                                new Point(60.000, 35.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(60.000, 35.000, Point.CARTESIAN),
-                                new Point(60.000, 25.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(60.000, 25.000, Point.CARTESIAN),
-                                new Point(15.000, 25.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(15.000, 25.000, Point.CARTESIAN),
-                                new Point(60.000, 25.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(60.000, 25.000, Point.CARTESIAN),
-                                new Point(60.000, 15.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(60.000, 15.000, Point.CARTESIAN),
-                                new Point(15.000, 15.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(15.000, 15.000, Point.CARTESIAN),
-                                new Point(60.000, 15.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(60.000, 15.000, Point.CARTESIAN),
-                                new Point(60.000, 8.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(60.000, 8.000, Point.CARTESIAN),
-                                new Point(15.000, 8.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleThree.java

@@ -1,89 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning")
-public class AutoExampleThree extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(10,45, 0);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.4);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierCurve(
-                                new Point(10.000, 45.000, Point.CARTESIAN),
-                                new Point(45.000, 45.000, Point.CARTESIAN),
-                                new Point(50.000, 20.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(50.000, 20.000, Point.CARTESIAN),
-                                new Point(10.000, 20.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
-                .build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleTwo.java

@@ -1,80 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning")
-public class AutoExampleTwo extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(10.0, 45, 0);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.4);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierCurve(
-                                new Point(10.000, 45.000, Point.CARTESIAN),
-                                new Point(45.000, 45.000, Point.CARTESIAN),
-                                new Point(50.000, 20.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
-                .build();
-
-        follower.followPath(path);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java

@@ -1,91 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous (name = "Test", group = "Autonomous Pathing Tuning")
-public class AutoTest extends OpMode {
-    private Telemetry telemetryA;
-
-
-    private Follower follower;
-
-    private PathChain test;
-
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        test = follower.pathBuilder()
-                .addPath(
-        new BezierLine(
-                new Point(8.000, 60.000, Point.CARTESIAN),
-                new Point(18.000, 60.000, Point.CARTESIAN)
-        )
-      )
-
-                .addPath(
-                        // Line 2
-                        new BezierCurve(
-                                new Point(18.000, 60.000, Point.CARTESIAN),
-                                new Point(18.000, 23.000, Point.CARTESIAN),
-                                new Point(48.000, 23.000, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(48.000, 23.000, Point.CARTESIAN),
-                                new Point(60.000, 36.000, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(60.000, 36.000, Point.CARTESIAN),
-                                new Point(60.000, 49.000, Point.CARTESIAN)
-                        )
-                ).build();
-
-
-        follower.followPath(test);
-
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-    }
-
-
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(test);
-        }
-
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java

@@ -1,128 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-public class BlueAuto {
-
-
-
-    public void GeneratedPath() {
-        PathBuilder builder = new PathBuilder();
-
-        builder
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(9.757, 84.983, Point.CARTESIAN),
-                                new Point(8.442, 129.227, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(8.442, 129.227, Point.CARTESIAN),
-                                new Point(52.762, 101.628, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(52.762, 101.628, Point.CARTESIAN),
-                                new Point(79.224, 116.564, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(79.224, 116.564, Point.CARTESIAN),
-                                new Point(54.548, 130.525, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(54.548, 130.525, Point.CARTESIAN),
-                                new Point(12.338, 133.772, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(12.338, 133.772, Point.CARTESIAN),
-                                new Point(52.437, 101.628, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(52.437, 101.628, Point.CARTESIAN),
-                                new Point(71.594, 120.947, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(71.594, 120.947, Point.CARTESIAN),
-                                new Point(52.275, 120.785, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(52.275, 120.785, Point.CARTESIAN),
-                                new Point(11.039, 131.012, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 10
-                        new BezierLine(
-                                new Point(11.039, 131.012, Point.CARTESIAN),
-                                new Point(70.782, 120.460, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 11
-                        new BezierLine(
-                                new Point(70.782, 120.460, Point.CARTESIAN),
-                                new Point(50.327, 142.377, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 12
-                        new BezierLine(
-                                new Point(50.327, 142.377, Point.CARTESIAN),
-                                new Point(13.799, 134.422, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 13
-                        new BezierLine(
-                                new Point(13.799, 134.422, Point.CARTESIAN),
-                                new Point(13.799, 134.422, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 14
-                        new BezierLine(
-                                new Point(13.799, 134.422, Point.CARTESIAN),
-                                new Point(71.919, 103.901, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation();
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java

@@ -1,246 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "BluebAutoV1", group = "Autonomous Pathing Tuning")
-public class BluebAutoV1 extends OpMode {
-    private Telemetry telemetryA;
-
-    private Follower follower;
-
-    private PathChain path;
-
-    private final Pose startPose = new Pose(7.5, 72, 90);
-
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
-    @Override
-    public void init() {
-        follower = new Follower(hardwareMap);
-
-        follower.setMaxPower(.4);
-
-        follower.setStartingPose(startPose);
-
-        path = follower.pathBuilder()
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(7.5, 72, Point.CARTESIAN),
-                                new Point(29.893, 38.250, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(29.893, 38.250, Point.CARTESIAN),
-                                new Point(65.250, 32.143, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(65.250, 32.143, Point.CARTESIAN),
-                                new Point(61.714, 24.429, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(61.714, 24.429, Point.CARTESIAN),
-                                new Point(13.821, 22.821, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(13.821, 22.821, Point.CARTESIAN),
-                                new Point(61.714, 24.429, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(61.714, 24.429, Point.CARTESIAN),
-                                new Point(60.750, 12.696, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(60.750, 12.696, Point.CARTESIAN),
-                                new Point(12.375, 13.179, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(12.375, 13.179, Point.CARTESIAN),
-                                new Point(60.750, 12.536, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(60.750, 12.536, Point.CARTESIAN),
-                                new Point(60.589, 9.321, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 10
-                        new BezierLine(
-                                new Point(60.589, 9.321, Point.CARTESIAN),
-                                new Point(12.536, 8.357, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 11
-                        new BezierLine(
-                                new Point(12.536, 8.357, Point.CARTESIAN),
-                                new Point(26.679, 8.679, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 12
-                        new BezierLine(
-                                new Point(26.679, 8.679, Point.CARTESIAN),
-                                new Point(22.821, 109.446, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 13
-                        new BezierLine(
-                                new Point(22.821, 109.446, Point.CARTESIAN),
-                                new Point(70.714, 109.446, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 14
-                        new BezierLine(
-                                new Point(70.714, 109.446, Point.CARTESIAN),
-                                new Point(71.036, 120.214, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 15
-                        new BezierLine(
-                                new Point(71.036, 120.214, Point.CARTESIAN),
-                                new Point(22.179, 120.214, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 16
-                        new BezierLine(
-                                new Point(22.179, 120.214, Point.CARTESIAN),
-                                new Point(11.089, 130.821, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 17
-                        new BezierLine(
-                                new Point(11.089, 130.821, Point.CARTESIAN),
-                                new Point(70.714, 112.018, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 18
-                        new BezierLine(
-                                new Point(70.714, 112.018, Point.CARTESIAN),
-                                new Point(70.714, 128.250, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 19
-                        new BezierLine(
-                                new Point(70.714, 128.250, Point.CARTESIAN),
-                                new Point(9.964, 130.018, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 20
-                        new BezierLine(
-                                new Point(9.964, 130.018, Point.CARTESIAN),
-                                new Point(70.554, 130.500, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 21
-                        new BezierLine(
-                                new Point(70.554, 130.500, Point.CARTESIAN),
-                                new Point(70.393, 135.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90))
-                .addPath(
-                        // Line 22
-                        new BezierLine(
-                                new Point(70.393, 135.000, Point.CARTESIAN),
-                                new Point(13.821, 134.839, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
-
-        follower.followPath(path);
-        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-        telemetryA.update();
-
-    }
-
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
-    @Override
-    public void loop() {
-        follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path);
-        }
-        follower.telemetryDebug(telemetryA);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluenbAutov1.java

@@ -1,4 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-public class BluenbAutov1 {
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java

@@ -1,109 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-public class GeneratedPath {
-    public GeneratedPath() {
-        PathBuilder builder = new PathBuilder();
-
-        builder
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(9.757, 84.983, Point.CARTESIAN),
-                                new Point(28.573, 76.302, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(28.573, 76.302, Point.CARTESIAN),
-                                new Point(36.203, 76.140, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(36.203, 76.140, Point.CARTESIAN),
-                                new Point(35.067, 35.716, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(35.067, 35.716, Point.CARTESIAN),
-                                new Point(73.705, 34.742, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(73.705, 34.742, Point.CARTESIAN),
-                                new Point(73.705, 24.839, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(73.705, 24.839, Point.CARTESIAN),
-                                new Point(7.630, 26.462, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(7.630, 26.462, Point.CARTESIAN),
-                                new Point(64.126, 22.728, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(64.126, 22.728, Point.CARTESIAN),
-                                new Point(63.964, 13.150, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(63.964, 13.150, Point.CARTESIAN),
-                                new Point(12.338, 15.260, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 10
-                        new BezierLine(
-                                new Point(12.338, 15.260, Point.CARTESIAN),
-                                new Point(63.802, 13.150, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 11
-                        new BezierLine(
-                                new Point(63.802, 13.150, Point.CARTESIAN),
-                                new Point(63.639, 11.689, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 12
-                        new BezierLine(
-                                new Point(63.639, 11.689, Point.CARTESIAN),
-                                new Point(12.014, 11.689, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation();
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java

@@ -1,99 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-
-
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-public class RedAuto {
-
-
-    public class GeneratedPath {
-
-        public GeneratedPath() {
-            PathBuilder builder = new PathBuilder();
-
-            builder
-                    .addPath(
-                            // Line 1
-                            new BezierLine(
-                                    new Point(131.499, 58.931, Point.CARTESIAN),
-                                    new Point(131.986, 18.183, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 2
-                            new BezierLine(
-                                    new Point(131.986, 18.183, Point.CARTESIAN),
-                                    new Point(90.264, 40.911, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 3
-                            new BezierLine(
-                                    new Point(90.264, 40.911, Point.CARTESIAN),
-                                    new Point(83.445, 26.300, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 4
-                            new BezierLine(
-                                    new Point(83.445, 26.300, Point.CARTESIAN),
-                                    new Point(136.207, 14.286, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 5
-                            new BezierLine(
-                                    new Point(136.207, 14.286, Point.CARTESIAN),
-                                    new Point(81.497, 24.352, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 6
-                            new BezierLine(
-                                    new Point(81.497, 24.352, Point.CARTESIAN),
-                                    new Point(82.634, 12.988, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 7
-                            new BezierLine(
-                                    new Point(82.634, 12.988, Point.CARTESIAN),
-                                    new Point(133.935, 11.364, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 8
-                            new BezierLine(
-                                    new Point(133.935, 11.364, Point.CARTESIAN),
-                                    new Point(82.309, 11.689, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 9
-                            new BezierLine(
-                                    new Point(82.309, 11.689, Point.CARTESIAN),
-                                    new Point(83.445, 2.598, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation()
-                    .addPath(
-                            // Line 10
-                            new BezierLine(
-                                    new Point(83.445, 2.598, Point.CARTESIAN),
-                                    new Point(132.149, 10.390, Point.CARTESIAN)
-                            )
-                    )
-                    .setTangentHeadingInterpolation();
-        }
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java

@@ -1,185 +0,0 @@
-/* Copyright (c) 2017 FIRST. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted (subject to the limitations in the disclaimer below) provided that
- * the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of FIRST nor the names of its contributors may be used to endorse or
- * promote products derived from this software without specific prior written permission.
- *
- * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
- * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.robotcore.external.Func;
-import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
-import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
-import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
-import org.firstinspires.ftc.robotcore.external.navigation.Position;
-import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
-
-import java.util.Locale;
-
-/*
- * This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit.
- *
- * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
- * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
- *
- * 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
- *
- * @see <a href="http://www.adafruit.com/products/2472">Adafruit IMU</a>
- */
-@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor")
-public class SensorBNO055IMU extends LinearOpMode
-    {
-    //----------------------------------------------------------------------------------------------
-    // State
-    //----------------------------------------------------------------------------------------------
-
-    // The IMU sensor object
-    BNO055IMU imu;
-
-    // State used for updating telemetry
-    Orientation angles;
-    Acceleration gravity;
-
-    //----------------------------------------------------------------------------------------------
-    // Main logic
-    //----------------------------------------------------------------------------------------------
-
-    @Override public void runOpMode() {
-
-        // Set up the parameters with which we will use our IMU. Note that integration
-        // algorithm here just reports accelerations to the logcat log; it doesn't actually
-        // provide positional information.
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-        parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
-        parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
-        parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode
-        parameters.loggingEnabled      = true;
-        parameters.loggingTag          = "IMU";
-        parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
-
-        // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
-        // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
-        // and named "imu".
-        imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu");
-        imu.initialize(parameters);
-
-        // Set up our telemetry dashboard
-        composeTelemetry();
-
-        // Wait until we're told to go
-        waitForStart();
-
-        // Start the logging of measured acceleration
-        imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);
-
-        // Loop and update the dashboard
-        while (opModeIsActive()) {
-            telemetry.update();
-        }
-    }
-
-    //----------------------------------------------------------------------------------------------
-    // Telemetry Configuration
-    //----------------------------------------------------------------------------------------------
-
-    void composeTelemetry() {
-
-        // At the beginning of each telemetry update, grab a bunch of data
-        // from the IMU that we will then display in separate lines.
-        telemetry.addAction(new Runnable() { @Override public void run()
-                {
-                // Acquiring the angles is relatively expensive; we don't want
-                // to do that in each of the three items that need that info, as that's
-                // three times the necessary expense.
-                angles   = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
-                gravity  = imu.getGravity();
-                }
-            });
-
-        telemetry.addLine()
-            .addData("status", new Func<String>() {
-                @Override public String value() {
-                    return imu.getSystemStatus().toShortString();
-                    }
-                })
-            .addData("calib", new Func<String>() {
-                @Override public String value() {
-                    return imu.getCalibrationStatus().toString();
-                    }
-                });
-
-        telemetry.addLine()
-            .addData("heading", new Func<String>() {
-                @Override public String value() {
-                    return formatAngle(angles.angleUnit, angles.firstAngle);
-                    }
-                })
-            .addData("roll", new Func<String>() {
-                @Override public String value() {
-                    return formatAngle(angles.angleUnit, angles.secondAngle);
-                    }
-                })
-            .addData("pitch", new Func<String>() {
-                @Override public String value() {
-                    return formatAngle(angles.angleUnit, angles.thirdAngle);
-                    }
-                });
-
-        telemetry.addLine()
-            .addData("grvty", new Func<String>() {
-                @Override public String value() {
-                    return gravity.toString();
-                    }
-                })
-            .addData("mag", new Func<String>() {
-                @Override public String value() {
-                    return String.format(Locale.getDefault(), "%.3f",
-                            Math.sqrt(gravity.xAccel*gravity.xAccel
-                                    + gravity.yAccel*gravity.yAccel
-                                    + gravity.zAccel*gravity.zAccel));
-                    }
-                });
-    }
-
-    //----------------------------------------------------------------------------------------------
-    // Formatting
-    //----------------------------------------------------------------------------------------------
-
-    String formatAngle(AngleUnit angleUnit, double angle) {
-        return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
-    }
-
-    String formatDegrees(double degrees){
-        return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java

@@ -1,229 +0,0 @@
-/* Copyright (c) 2017 FIRST. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted (subject to the limitations in the disclaimer below) provided that
- * the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of FIRST nor the names of its contributors may be used to endorse or
- * promote products derived from this software without specific prior written permission.
- *
- * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
- * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.util.ReadWriteFile;
-
-import org.firstinspires.ftc.robotcore.external.Func;
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
-import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
-import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
-import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
-
-import java.io.File;
-import java.util.Locale;
-
-/*
- * This OpMode calibrates a BNO055 IMU per
- * "Section 3.11 Calibration" of the BNO055 specification.
- *
- *  Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
- *  Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
- *
- * Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer (which is not used by the
- * default "IMU" SensorMode), the BNO055 is internally self-calibrating and thus can be very successfully used without
- * manual intervention. That said, performing a one-time calibration, saving the results persistently, then loading them
- * again at each run can help reduce the time that automatic calibration requires.
- *
- * This summary of the calibration process from Intel is informative:
- * http://iotdk.intel.com/docs/master/upm/classupm_1_1_b_n_o055.html
- *
- * "This device requires calibration in order to operate accurately. [...] Calibration data is
- * lost on a power cycle. See one of the examples for a description of how to calibrate the device,
- * but in essence:
- *
- * There is a calibration status register available [...] that returns the calibration status
- * of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS).
- * Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally]
- * involves certain motions to get all 4 values at 3. The motions are as follows (though see the
- * datasheet for more information):
- *
- *     1. GYR: Simply let the sensor sit flat for a few seconds.</ol>
- *     2. ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45
- *             degrees, hold for a few seconds, then continue rotating another 45 degrees and
- *             hold, etc. 6 or more movements of this type may be required. You can move through
- *             any axis you desire, but make sure that the device is lying at least once
- *             perpendicular to the x, y, and z axis.</ol>
- *     3. MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3.</ol>
- *     4. SYS: This will usually reach 3 when the other items have also reached 3. If not, continue
- *             slowly moving the device though various axes until it does."</ol>
- *
- * To calibrate the IMU, run this sample OpMode with a gamepad attached to the driver station.
- * Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A'
- * button on the gamepad to write the calibration to a file. That file can then be indicated
- * later when running an OpMode which uses the IMU.
- *
- * Note: if your intended uses of the IMU do not include use of all its sensors (for example,
- * you might not use the magnetometer), then it makes little sense for you to wait for full
- * calibration of the sensors you are not using before saving the calibration data. Indeed,
- * it appears that in a SensorMode that doesn't use the magnetometer (for example), the
- * magnetometer cannot actually be calibrated.
- *
- * References:
- * The AdafruitBNO055IMU Javadoc
- * The BNO055IMU.Parameters.calibrationDataFile Javadoc
- * The BNO055 product page: https://www.bosch-sensortec.com/bst/products/all_products/bno055
- * The BNO055 datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
- */
-@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor")
-public class SensorBNO055IMUCalibration extends LinearOpMode
-    {
-    //----------------------------------------------------------------------------------------------
-    // State
-    //----------------------------------------------------------------------------------------------
-
-    // Our sensors, motors, and other devices go here, along with other long term state
-    BNO055IMU imu;
-
-    // State used for updating telemetry
-    Orientation angles;
-
-    //----------------------------------------------------------------------------------------------
-    // Main logic
-    //----------------------------------------------------------------------------------------------
-
-    @Override public void runOpMode() {
-
-        telemetry.log().setCapacity(12);
-        telemetry.log().add("");
-        telemetry.log().add("Please refer to the calibration instructions");
-        telemetry.log().add("contained in the Adafruit IMU calibration");
-        telemetry.log().add("sample OpMode.");
-        telemetry.log().add("");
-        telemetry.log().add("When sufficient calibration has been reached,");
-        telemetry.log().add("press the 'A' button to write the current");
-        telemetry.log().add("calibration data to a file.");
-        telemetry.log().add("");
-
-        // We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu".
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-        parameters.loggingEnabled = true;
-        parameters.loggingTag     = "IMU";
-        imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu");
-        imu.initialize(parameters);
-
-        composeTelemetry();
-        telemetry.log().add("Waiting for start...");
-
-        // Wait until we're told to go
-        while (!isStarted()) {
-            telemetry.update();
-            idle();
-        }
-
-        telemetry.log().add("...started...");
-
-        while (opModeIsActive()) {
-
-            if (gamepad1.a) {
-
-                // Get the calibration data
-                BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData();
-
-                // Save the calibration data to a file. You can choose whatever file
-                // name you wish here, but you'll want to indicate the same file name
-                // when you initialize the IMU in an OpMode in which it is used. If you
-                // have more than one IMU on your robot, you'll of course want to use
-                // different configuration file names for each.
-                String filename = "AdafruitIMUCalibration.json";
-                File file = AppUtil.getInstance().getSettingsFile(filename);
-                ReadWriteFile.writeFile(file, calibrationData.serialize());
-                telemetry.log().add("saved to '%s'", filename);
-
-                // Wait for the button to be released
-                while (gamepad1.a) {
-                    telemetry.update();
-                    idle();
-                }
-            }
-
-            telemetry.update();
-        }
-    }
-
-    void composeTelemetry() {
-
-        // At the beginning of each telemetry update, grab a bunch of data
-        // from the IMU that we will then display in separate lines.
-        telemetry.addAction(new Runnable() { @Override public void run()
-                {
-                // Acquiring the angles is relatively expensive; we don't want
-                // to do that in each of the three items that need that info, as that's
-                // three times the necessary expense.
-                angles   = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
-                }
-            });
-
-        telemetry.addLine()
-            .addData("status", new Func<String>() {
-                @Override public String value() {
-                    return imu.getSystemStatus().toShortString();
-                    }
-                })
-            .addData("calib", new Func<String>() {
-                @Override public String value() {
-                    return imu.getCalibrationStatus().toString();
-                    }
-                });
-
-        telemetry.addLine()
-            .addData("heading", new Func<String>() {
-                @Override public String value() {
-                    return formatAngle(angles.angleUnit, angles.firstAngle);
-                    }
-                })
-            .addData("roll", new Func<String>() {
-                @Override public String value() {
-                    return formatAngle(angles.angleUnit, angles.secondAngle);
-                    }
-                })
-            .addData("pitch", new Func<String>() {
-                @Override public String value() {
-                    return formatAngle(angles.angleUnit, angles.thirdAngle);
-                    }
-                });
-    }
-
-    //----------------------------------------------------------------------------------------------
-    // Formatting
-    //----------------------------------------------------------------------------------------------
-
-    String formatAngle(AngleUnit angleUnit, double angle) {
-        return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
-    }
-
-    String formatDegrees(double degrees){
-        return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java

@@ -1,172 +0,0 @@
-/* Copyright (c) 2022 FIRST. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted (subject to the limitations in the disclaimer below) provided that
- * the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this list
- * of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice, this
- * list of conditions and the following disclaimer in the documentation and/or
- * other materials provided with the distribution.
- *
- * Neither the name of FIRST nor the names of its contributors may be used to endorse or
- * promote products derived from this software without specific prior written permission.
- *
- * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
- * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_LOGO_FACING_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
-
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.IMU;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-import org.firstinspires.ftc.teamcode.PedroConstants;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
-
-/*
- * This OpMode shows how to use the new universal IMU interface. This
- * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
- * on the robot with the name "imu".
- *
- * The sample will display the current Yaw, Pitch and Roll of the robot.<br>
- * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
- *   Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
- *   Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
- *   Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
- *
- * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
- *
- * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
- * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
- *
- * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
- *       the alternative SensorIMUNonOrthogonal sample in this folder.
- *
- * This "Orthogonal" requirement means that:
- *
- * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
- *    FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
- *
- * 2) The USB ports can only be pointing in one of the same six directions:<br>
- *    FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
- *
- * So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
- *    logoFacingDirection<br>
- *    usbFacingDirection
- *
- * 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.
- *
- * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
- * to use those parameters.
- */
-@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
-@Disabled   // Comment this out to add to the OpMode list
-public class SensorIMUOrthogonal extends LinearOpMode {
-    // The IMU sensor object
-    IMU imu;
-    private Encoder leftEncoder;
-    private Encoder rightEncoder;
-    private Encoder strafeEncoder;
-
-    //----------------------------------------------------------------------------------------------
-    // Main logic
-    //----------------------------------------------------------------------------------------------
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-        // Retrieve and initialize the IMU.
-        // This sample expects the IMU to be in a REV Hub and named "imu".
-        imu = hardwareMap.get(IMU.class, PedroConstants.IMU);
-
-        // TODO: replace these with your encoder ports
-        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
-        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
-        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
-
-        // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
-        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
-        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
-
-        /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
-         *
-         * Two input parameters are required to fully specify the Orientation.
-         * The first parameter specifies the direction the printed logo on the Hub is pointing.
-         * The second parameter specifies the direction the USB connector on the Hub is pointing.
-         * All directions are relative to the robot, and left/right is as-viewed from behind the robot.
-         *
-         * If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
-         * RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
-         */
-
-        /* The next two lines define Hub orientation.
-         * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
-         *
-         * To Do:  EDIT these two lines to match YOUR mounting configuration.
-         */
-        RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION;
-        RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION;
-
-        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
-
-        // Now initialize the IMU with this mounting orientation
-        // Note: if you choose two conflicting directions, this initialization will cause a code exception.
-        imu.initialize(new IMU.Parameters(orientationOnRobot));
-
-        // Loop and update the dashboard
-        while (!isStopRequested()) {
-
-            telemetry.addData("Hub orientation", "Logo=%s   USB=%s\n ", logoDirection, usbDirection);
-
-            // Check to see if heading reset is requested
-            if (gamepad1.y) {
-                telemetry.addData("Yaw", "Resetting\n");
-                imu.resetYaw();
-            } else {
-                telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
-            }
-
-            // Retrieve Rotational Angles and Velocities
-            YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
-            AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
-
-            telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
-            telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
-            telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
-            telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
-            telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
-            telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
-            telemetry.update();
-        }
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java

@@ -1,237 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-@Autonomous(name = "org.firstinspires.ftc.teamcode.cometbots.projects.bBlueAutoV1", group = "V1")
-public class bBlueAutoV1 extends OpMode {
-
-    public Telemetry telemetry;
-    public Follower robot;
-    public PathChain path;
-
-    @Override
-    public void init() {
-        robot = new Follower(hardwareMap);
-
-        PathBuilder builder = new PathBuilder();
-
-       path = builder
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(9.757, 84.983, Point.CARTESIAN),
-                                new Point(28.573, 76.302, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(28.573, 76.302, Point.CARTESIAN),
-                                new Point(36.203, 76.140, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(36.203, 76.140, Point.CARTESIAN),
-                                new Point(35.067, 35.716, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(35.067, 35.716, Point.CARTESIAN),
-                                new Point(73.705, 34.742, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(73.705, 34.742, Point.CARTESIAN),
-                                new Point(73.705, 24.839, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(73.705, 24.839, Point.CARTESIAN),
-                                new Point(7.630, 26.462, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(7.630, 26.462, Point.CARTESIAN),
-                                new Point(64.126, 22.728, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(64.126, 22.728, Point.CARTESIAN),
-                                new Point(63.964, 13.150, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(63.964, 13.150, Point.CARTESIAN),
-                                new Point(12.338, 15.260, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 10
-                        new BezierLine(
-                                new Point(12.338, 15.260, Point.CARTESIAN),
-                                new Point(63.802, 13.150, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 11
-                        new BezierLine(
-                                new Point(63.802, 13.150, Point.CARTESIAN),
-                                new Point(63.639, 11.689, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 12
-                        new BezierLine(
-                                new Point(63.639, 11.689, Point.CARTESIAN),
-                                new Point(12.014, 11.689, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 13
-                        new BezierLine(
-                                new Point(12.014, 11.689, Point.CARTESIAN),
-                                new Point(62.665, 30.196, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 14
-                        new BezierLine(
-                                new Point(62.665, 30.196, Point.CARTESIAN),
-                                new Point(13.312, 51.463, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 15
-                        new BezierLine(
-                                new Point(13.312, 51.463, Point.CARTESIAN),
-                                new Point(16.234, 103.738, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 16
-                        new BezierLine(
-                                new Point(16.234, 103.738, Point.CARTESIAN),
-                                new Point(68.023, 108.284, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 17
-                        new BezierLine(
-                                new Point(68.023, 108.284, Point.CARTESIAN),
-                                new Point(68.185, 121.109, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 18
-                        new BezierLine(
-                                new Point(68.185, 121.109, Point.CARTESIAN),
-                                new Point(21.754, 119.811, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 19
-                        new BezierLine(
-                                new Point(21.754, 119.811, Point.CARTESIAN),
-                                new Point(11.526, 129.227, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 20
-                        new BezierLine(
-                                new Point(11.526, 129.227, Point.CARTESIAN),
-                                new Point(72.568, 111.856, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 21
-                        new BezierLine(
-                                new Point(72.568, 111.856, Point.CARTESIAN),
-                                new Point(58.607, 128.902, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 22
-                        new BezierLine(
-                                new Point(58.607, 128.902, Point.CARTESIAN),
-                                new Point(11.364, 130.850, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 23
-                        new BezierLine(
-                                new Point(11.364, 130.850, Point.CARTESIAN),
-                                new Point(58.931, 128.577, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 24
-                        new BezierLine(
-                                new Point(58.931, 128.577, Point.CARTESIAN),
-                                new Point(58.769, 133.123, Point.CARTESIAN)
-                        )
-                )
-
-                .addPath(
-                        // Line 25
-                        new BezierLine(
-                                new Point(58.769, 133.123, Point.CARTESIAN),
-                                new Point(13.475, 133.935, Point.CARTESIAN)
-                        )
-                ).build();
-        ;
-    }
-
-
-    @Override
-    public void loop() {
-        robot.update();
-        if (robot.atParametricEnd())
-            robot.followPath(path);
-        robot.telemetryDebug(telemetry);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java

@@ -1,174 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-public class bRedAutoV1 {
-
-    public bRedAutoV1() {
-        PathBuilder builder = new PathBuilder();
-
-        builder
-                .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(133.935, 83.770, Point.CARTESIAN),
-                                new Point(79.874, 117.213, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 2
-                        new BezierLine(
-                                new Point(79.874, 117.213, Point.CARTESIAN),
-                                new Point(79.874, 120.785, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 3
-                        new BezierLine(
-                                new Point(79.874, 120.785, Point.CARTESIAN),
-                                new Point(131.824, 118.349, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 4
-                        new BezierLine(
-                                new Point(131.824, 118.349, Point.CARTESIAN),
-                                new Point(79.549, 120.460, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 5
-                        new BezierLine(
-                                new Point(79.549, 120.460, Point.CARTESIAN),
-                                new Point(79.549, 128.740, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 6
-                        new BezierLine(
-                                new Point(79.549, 128.740, Point.CARTESIAN),
-                                new Point(131.337, 128.090, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 7
-                        new BezierLine(
-                                new Point(131.337, 128.090, Point.CARTESIAN),
-                                new Point(79.549, 128.740, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 8
-                        new BezierLine(
-                                new Point(79.549, 128.740, Point.CARTESIAN),
-                                new Point(79.549, 133.610, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(79.549, 133.610, Point.CARTESIAN),
-                                new Point(130.850, 133.285, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 10
-                        new BezierLine(
-                                new Point(130.850, 133.285, Point.CARTESIAN),
-                                new Point(130.201, 36.528, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 11
-                        new BezierLine(
-                                new Point(130.201, 36.528, Point.CARTESIAN),
-                                new Point(84.095, 36.203, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 12
-                        new BezierLine(
-                                new Point(84.095, 36.203, Point.CARTESIAN),
-                                new Point(84.095, 23.378, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 13
-                        new BezierLine(
-                                new Point(84.095, 23.378, Point.CARTESIAN),
-                                new Point(119.811, 23.378, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 14
-                        new BezierLine(
-                                new Point(119.811, 23.378, Point.CARTESIAN),
-                                new Point(127.603, 13.475, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 15
-                        new BezierLine(
-                                new Point(127.603, 13.475, Point.CARTESIAN),
-                                new Point(88.640, 28.248, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 16
-                        new BezierLine(
-                                new Point(88.640, 28.248, Point.CARTESIAN),
-                                new Point(87.666, 15.910, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 17
-                        new BezierLine(
-                                new Point(87.666, 15.910, Point.CARTESIAN),
-                                new Point(127.603, 12.014, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 18
-                        new BezierLine(
-                                new Point(127.603, 12.014, Point.CARTESIAN),
-                                new Point(86.692, 12.825, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 19
-                        new BezierLine(
-                                new Point(86.692, 12.825, Point.CARTESIAN),
-                                new Point(86.692, 10.390, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation()
-                .addPath(
-                        // Line 20
-                        new BezierLine(
-                                new Point(86.692, 10.390, Point.CARTESIAN),
-                                new Point(126.467, 9.903, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation();
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java

@@ -0,0 +1,95 @@
+/* Copyright (c) 2021 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+
+@TeleOp(name = "Arm Test v2", group = "Debug")
+public class ArmTest extends LinearOpMode {
+
+    // Declare OpMode members for each of the 4 motors.
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Arm
+         */
+        ArmSubsystem arm = new ArmSubsystem(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        arm.initTeleOp();
+        waitForStart();
+        runtime.reset();
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            if (currentGamepad1.circle && !previousGamepad1.circle) {
+                arm.toParkPosition();
+            }
+
+            if (currentGamepad1.square && !previousGamepad1.square) {
+                arm.toBucketPosition();
+            }
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                arm.toFloorPosition();
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                arm.setPosition(arm.getPosition() - .05);
+            }
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+                arm.setPosition(arm.getPosition() + .05);
+            }
+
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Arm State", arm.getState());
+            telemetry.addData("Arm Position", arm.getPosition());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java

@@ -0,0 +1,85 @@
+/* Copyright (c) 2021 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+
+
+@TeleOp(name = "Claw Test v2", group = "Debug")
+public class ClawTest extends LinearOpMode {
+
+    // Declare OpMode members for each of the 4 motors.
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Claw
+         */
+        ClawSubsystem claw = new ClawSubsystem(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        waitForStart();
+        claw.init();
+        runtime.reset();
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                claw.switchState();
+            }
+
+            if (currentGamepad1.triangle && !previousGamepad1.triangle) {
+                claw.switchTState();
+            }
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Claw State", claw.getState());
+            telemetry.addData("Claw Thumb State", claw.getTState());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/DualMotorSliderTest.java

@@ -0,0 +1,72 @@
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+
+@Config
+@Autonomous(name = "Lift Motor Subsystem - PID Test")
+public class DualMotorSliderTest extends LinearOpMode {
+
+    private DcMotorEx liftSlideLeft;
+    private DcMotorEx liftSlideRight;
+    public static double kp = 0.002, ki = 0, kd = 0;
+    private double lastError = 0;
+    private double integralSum = 0;
+    public static int targetPosition = 0;
+    private final FtcDashboard dashboard = FtcDashboard.getInstance();
+    private ElapsedTime timer = new ElapsedTime();
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        TelemetryPacket packet = new TelemetryPacket();
+        dashboard.setTelemetryTransmissionInterval(25);
+
+        liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
+        liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
+
+        liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        waitForStart();
+
+        while(opModeIsActive()) {
+            double power = calculatePower(targetPosition, liftSlideLeft.getCurrentPosition());
+
+            packet.put("Power", power);
+            packet.put("Position", liftSlideLeft.getCurrentPosition());
+            packet.put("Error", lastError);
+            packet.put("Seconds", timer.seconds());
+
+            liftSlideLeft.setPower(power);
+            liftSlideRight.setPower(power);
+
+            dashboard.sendTelemetryPacket(packet);
+        }
+    }
+
+    private double calculatePower(int targetPosition, int currentPosition) {
+        // reference is targetPosition, state is currentPosition
+        double error = targetPosition - currentPosition;
+        integralSum += error * timer.seconds();
+        double derivative = (error - lastError) / timer.seconds();
+        lastError = error;
+        timer.reset();
+        return (error * kp) + (derivative * kd) + (integralSum * ki);
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java

@@ -0,0 +1,92 @@
+/* Copyright (c) 2021 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+
+@TeleOp(name = "Wrist Test v2", group = "Debug")
+public class WristTest extends LinearOpMode {
+
+    // Declare OpMode members for each of the 4 motors.
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Wrist
+         */
+        WristSubsystem wrist = new WristSubsystem(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        wrist.initTeleOp();
+        waitForStart();
+        runtime.reset();
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            if (currentGamepad1.square && !previousGamepad1.square) {
+                wrist.toBucketPosition();
+            }
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                wrist.switchState();
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                wrist.setPosition(wrist.getPosition() - .05);
+            }
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+                wrist.setPosition(wrist.getPosition() + .05);
+            }
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Wrist State", wrist.getState());
+            telemetry.addData("Wrist Position", wrist.getPosition());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.configs;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class RobotConstants {
+    public final static double clawClose = 0.95;
+    public final static double clawOpen = 0.35;
+    public final static int autoClawOpen = 0;
+
+
+    public final static double armFloor = 0.7;
+    public final static double armSubmarine = 0.55;
+    public final static double armReverseBucket = 0.08;
+    public final static double armPark = 0.33;
+    //value for grabbing the hook Specimen
+    public final static double grabBlueberry = 0.56;
+    public final static double armGrabBlueberrySkyhook = 0.045;
+    public final static double wristGrabBlueberrySkyhook = 0.08;
+    public final static double armHangBlueberrySkyhook = 0.18;
+    public final static double wristHangBlueberrySkyhook = 0;
+    public final static int slideHangBlueberrySkyhook = 500;
+
+    public final static int slideBelowFloor = -150;
+    public final static int slideSpecimanHang = 900;
+    public final static int slideSpecimanRelease = 200;
+    public final static int backwardBucketDrop = 4670;
+    public final static double armBucket = 0.45;
+    public final static double armSpecimen = 0.155;
+    public final static double armInit = 0.13;
+    public final static double wristInit = 0;
+    public final static double wristPickup = 0.425;
+    public final static double wristBucket = 0.56;
+    public final static double wristSpecimenPrep = 0.63;
+    public final static double wristSpecimenHang = 0.53;
+
+    public final static double wristFloor = 0.75;
+    public final static double wristBackwardBucket = 0.725;
+
+    public final static double wristRung = 0.55;
+
+    public final static double wristSpeciemen = 0.1;
+
+    public final static int toBar = -7000;
+    public final static double wristtravel = 0.525;
+
+    public final static int toFloor = 0;
+    public final static int liftToFloorPos = 350;
+    public final static int liftToSubmarinePos = 350;
+    public final static int liftToLowBucketPos = 1750;
+    public final static int liftToHighRung = 2100;
+    public final static int dropToHighRung = 1675;
+
+    public final static int liftToHighRungAttach = 1050;
+
+
+    public final static int liftToHighBucketPos = 4500;
+    public final static double liftPower = 1;
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/LOCALIZATION.md

@@ -4,10 +4,10 @@ the pose exponential method of localization. It's basically a way of turning mov
 robot's coordinate frame to the global coordinate frame. If you're interested in reading more about
 it, then check out pages 177 - 183 of [Controls Engineering in the FIRST Robotics Competition](https://file.tavsys.net/control/controls-engineering-in-frc.pdf)
 by Tyler Veness. However, the OTOS localizer uses its own onboard system for calculating localization,
-which I do not know about.
+which we do not know about.
 
 ## Setting Your Localizer
-Go to line `70` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)`
+Go to line `73` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)`
 with the localizer that applies to you:
 * If you're using drive encoders, put `new DriveEncoderLocalizer(hardwareMap)`
 * If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)`
@@ -15,6 +15,7 @@ with the localizer that applies to you:
   don't change it from the default
 * If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)`
 * If you're using OTOS, put `new OTOSLocalizer(hardwareMap)`
+* If you're using Pinpoint, put `new PinpointLocalizer(hardwareMap)`
 
 ## Tuning
 To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive
@@ -241,6 +242,23 @@ that applies to you and follow the directions there.
   robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll
   want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer!
 
+## Pinpoint Localizer
+* First you will need to plug in the pinpoint to the i2c port. Ensure that the dead wheel encoder wires are
+  plugged into the proper ports on the pinpoint to ensure no error within the tuning steps.
+* Then, go to the `PinpointLocalier.java` file and go to where it tells you to replace
+  the current statement with your pinpoint port in the constructor. Replace the `deviceName` parameter
+  with the name of the port that the pinpoint is connected to.
+* Next, follow the instructions left by the TODO: comment and enter in the odometry measurements either in
+  mms or inches (We have the conversion rates listed).
+* First, to ensure that your pinpoint is properly connected, please run the `SensorGoBildaPinpointExample.java`
+  file left in the `tuning` folder located within `localization`.
+* Once completed, the localizer should be properly tuned. To test it out, you can go to
+  `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash)
+  and on the top right, switch the drop down from the default view to the field view. Then, on the bottom
+  left corner, you should see a field and the robot being drawn on the field. You can then move your
+  robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll
+  want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer!
+
 ## Using Road Runner's Localizer
 Of course, many teams have experience using Road Runner in the past and so have localizers from Road
 Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/OVERVIEW.md

@@ -19,7 +19,7 @@ Why use Pedro Pathing? Why not something else like Road Runner or Pure Pursuit?
   * Pure Pursuit searches for the farthest point on the path that's within a certain radius from the robot. Pure Pursuit will then go in a straight line to that point. This poses several problems, as a small search radius will cause some oscillations on corners, and a large search radius will cut corners on paths, which makes the paths inaccurate to real life.
   * Pedro Pathing instead corrects to the closest point on the path while still following the path. This ensures that the follower will stay on the path while still being able to move forward along the path without cutting corners or encountering oscillation issues.
 * Why not Road Runner?
-  * Road Runner is a motion profile based follower, which means that a set of instructions for motor powers are calculated for each path beforehand and then run. After reaching the end of this motion profile, Road Runner corrects. This can be sufficient for most situations, but if the robot encounters an obstacle or wheel slippage, it may be unable to correct in time.
+  * Road Runner is a motion profile based follower, which means that a set of instructions for motor powers are calculated for each path beforehand and then run. During this motion profile, Road Runner can struggle to correct. This can be sufficient for many situations, but if the robot encounters an obstacle or wheel slippage, it may be unable to correct in time.
   * Pedro Pathing instead dynamically corrects throughout the path. The movement vectors are calculated at every point along the path, and because of this, the path can even be changed midway through and Pedro Pathing will still be able to correct. Since correction occurs throughout the path, the error correction isn't concentrated on the end of the path and therefore the robot is able to better minimize error.
 
 ## How Does Pedro Path?
@@ -105,4 +105,5 @@ of the curvature formula, we can estimate a centripetal force correction and app
 control.
 
 ## Questions?
-If you still have more questions, feel free to contact us at `scottsbots10158@gmail.com`
+If you still have more questions, feel free to contact us at `scottsbots10158@gmail.com` or
+within our discord linked here(https://discord.gg/2GfC4qBP5s)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md

@@ -14,45 +14,56 @@ that the tuners require you to push the robot or the tuners output will say "inc
 measurements will be in centimeters.
 
 ## Tuning
-* To start with, we need the mass of the robot in kg. This is used for the centripetal force correction,
-  and the mass, with the variable name `mass`, should be put on line `86` in the `FollowerConstants`
+* First, make sure that your motor names and directions, located at the top of `FollowerConstants`,
+  are correct.
+
+* After that, we need the mass of the robot in kg. This is used for the centripetal force correction,
+  and the mass, with the variable name `mass`, should be put on line `92` in the `FollowerConstants`
   class under the `tuning` package.
 
 * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a
   45 degree angle from the forward direction, but the actual direction the force is output is actually
-  closer to forward. To find the direction your wheels will go, you will need to run the
+  closer to forward. Before running any OpModes, make sure your motors are reversed properly in the
+  `Follower` class constructor. To find the direction your wheels will go, you will need to run the
   `Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full
   power for 40 inches forward and to the right, respectively. The distance can be changed through FTC
   Dashboard under the dropdown for each respective class, but higher distances work better. After the
   distance has finished running, the end velocity will be output to telemetry. The robot may continue
   to drift a little bit after the robot has finished running the distance, so make sure you have
-  plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `33` in
-  the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `34` in the
+  plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `39` in
+  the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `40` in the
   `FollowerConstants` class. The variable names should be `xMovement` and `yMovement`, respectively.
 
 * The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These
   find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to
   get a more accurate estimation of the drive vector. To find this, you will need to run the
   `Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes.
-  These will run your robot until it hits a velocity of 10 inches/second forward and to the right,
+  These will run your robot until it hits a velocity of 30 inches/second forward and to the right,
   respectively. The velocity can be changed through FTC Dashboard under the dropdown for each
   respective class, but higher velocities work better. After the velocity has been reached, power will
   be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at
   which point it will display the deceleration in telemetry. This robot will need to drift to a stop
   to properly work, and the higher the velocity the greater the drift distance, so make sure you have
   enough room. Once you're done, put the zero power acceleration for the
-  `Forward Zero Power Acceleration Tuner` on line `94` in the `FollowerConstants` class and the zero
-  power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `98` in the
+  `Forward Zero Power Acceleration Tuner` on line `100` in the `FollowerConstants` class and the zero
+  power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `104` in the
   `FollowerConstants` class. The variable names should be `forwardZeroPowerAcceleration` and
   `lateralZeroPowerAcceleration`, respectively.
 
 * After this, we will want to tune the translational PID. Go to FTC Dashboard and disable all but
-  the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make
-  sure you disable the timer on autonomous OpModes. The PID for the translational error is called
-  `translationalPIDF`.  If you need to add a feedforward value, use the `translationalPIDFFeedForward`
-  since that will add the feedforward in the direction the robot is trying to move, rather than the
-  feedforward in the PIDF itself, since those will only add the feedforward one way. You can change
-  the PIDF constants and feedforward values, under the `FollowerConstants` tab in FTC Dashboard.
+  the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`.
+  Make sure you disable the timer on autonomous OpModes. You will notice in telemetry a message saying
+  that the robot will travel a distance forward and backward, this will not happen until later, so for
+  now you can ignore this message. The robot should not move when you run the opmode initally. Instead,
+  it should correct when you push it away from its   starting position. Note that this correction should
+  happen regardless of the robot's rotation, and   that the robot should not rotate itself (if it does,
+  disable `useHeading` as mentioned prior). Also note that the robot will only correct to an imaginary line
+  that runs straight forward from the robot's starting position, meaning that it will not correct in the
+  (original) forward direction. The PID for the translational error is called `translationalPIDF`.
+  If you need to add a feedforward value, use the `translationalPIDFFeedForward` since that will add
+  the feedforward in the direction the robot is trying to move, rather than the feedforward in the
+  PIDF itself, since those will only add the feedforward one way. You can change   the PIDF constants
+  and feedforward values, under the `FollowerConstants` tab in FTC Dashboard.
   To tune the PID, push the robot off the path and see how corrects. You will want to alternate sides
   you push to reduce field wear and tear as well as push with varying power and distance. I would 
   recommend tuning the PID so that it is capable of correcting while minimizing oscillations and still
@@ -70,9 +81,8 @@ measurements will be in centimeters.
   `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor
   of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
   what works best for you is most important. Higher numbers will cause a faster brake, but increase
-  oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
-  `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more 
-* sensitive than the others. For reference,
+  oscillations at the end. Lower numbers will do the opposite. This can be found on line `113` in
+  `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference,
   my P values were in the hundredths and thousandths place values, and my D values were in the hundred
   thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
   `useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
@@ -97,7 +107,7 @@ measurements will be in centimeters.
 * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open
   up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth`
   and turn off its timer. If you notice the robot is correcting towards the inside of the curve
-  as/after running a path, then increase `centripetalScaling`, which can be found on line `89` of
+  as/after running a path, then increase `centripetalScaling`, which can be found on line `95` of
   `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease
   `centripetalScaling`.
 
@@ -110,11 +120,11 @@ measurements will be in centimeters.
 ## Note About the PIDs
 In versions of Pedro Pathing before early August 2024, there were 2 PIDs used in the translational,
 heading, and drive control. However, now there is only one main PID. The old system can still be used.
-Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `157` to `159`
+Scroll down to the bottom of `FollowerConstants` and set all the booleans from lines `163` to `165`
 to true. They should be named `useSecondaryTranslationalPID`, `useSecondaryHeadingPID`, and `useSecondaryDrivePID`.
 This will enable the two PID system that Pedro Pathing originally used. From there, scroll
 down and all the values pertaining to the secondary PIDs will be there. The two PID system works with
 a PID that handles larger errors (the main PID) and a second PID to handle smaller errors (the
 secondary PID). The main PID should be tuned to move the error within the secondary PID's range
 without providing too much momentum that could cause an overshoot. The secondary PID should be tuned
-to correct within its range quickly and accurately while minimizing oscillations.
+to correct within its range quickly and accurately while minimizing oscillations.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java

@@ -15,6 +15,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 public class DriveVectorScaler {
     // This is ordered left front, left back, right front, right back. These are also normalized.
     private Vector[] mecanumVectors;
+    private double maxPowerScaling = 1;
 
     /**
      * This creates a new DriveVectorScaler, which takes in various movement vectors and outputs
@@ -52,9 +53,9 @@ public class DriveVectorScaler {
      */
     public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) {
         // clamps down the magnitudes of the input vectors
-        if (correctivePower.getMagnitude() > 1) correctivePower.setMagnitude(1);
-        if (headingPower.getMagnitude() > 1) headingPower.setMagnitude(1);
-        if (pathingPower.getMagnitude() > 1) pathingPower.setMagnitude(1);
+        if (correctivePower.getMagnitude() > maxPowerScaling) correctivePower.setMagnitude(maxPowerScaling);
+        if (headingPower.getMagnitude() > maxPowerScaling) headingPower.setMagnitude(maxPowerScaling);
+        if (pathingPower.getMagnitude() > maxPowerScaling) pathingPower.setMagnitude(maxPowerScaling);
 
         // the powers for the wheel vectors
         double [] wheelPowers = new double[4];
@@ -65,8 +66,8 @@ public class DriveVectorScaler {
         // this contains the pathing vectors, one for each side (heading control requires 2)
         Vector[] truePathingVectors = new Vector[2];
 
-        if (correctivePower.getMagnitude() == 1) {
-            // checks for corrective power equal to 1 in magnitude. if equal to one, then set pathing power to that
+        if (correctivePower.getMagnitude() == maxPowerScaling) {
+            // checks for corrective power equal to max power scaling in magnitude. if equal, then set pathing power to that
             truePathingVectors[0] = MathFunctions.copyVector(correctivePower);
             truePathingVectors[1] = MathFunctions.copyVector(correctivePower);
         } else {
@@ -74,7 +75,7 @@ public class DriveVectorScaler {
             Vector leftSideVector = MathFunctions.subtractVectors(correctivePower, headingPower);
             Vector rightSideVector = MathFunctions.addVectors(correctivePower, headingPower);
 
-            if (leftSideVector.getMagnitude() > 1 || rightSideVector.getMagnitude() > 1) {
+            if (leftSideVector.getMagnitude() > maxPowerScaling || rightSideVector.getMagnitude() > maxPowerScaling) {
                 //if the combined corrective and heading power is greater than 1, then scale down heading power
                 double headingScalingFactor = Math.min(findNormalizingScaling(correctivePower, headingPower), findNormalizingScaling(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, -1)));
                 truePathingVectors[0] = MathFunctions.subtractVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor));
@@ -84,7 +85,7 @@ public class DriveVectorScaler {
                 Vector leftSideVectorWithPathing = MathFunctions.addVectors(leftSideVector, pathingPower);
                 Vector rightSideVectorWithPathing = MathFunctions.addVectors(rightSideVector, pathingPower);
 
-                if (leftSideVectorWithPathing.getMagnitude() > 1 || rightSideVectorWithPathing.getMagnitude() > 1) {
+                if (leftSideVectorWithPathing.getMagnitude() > maxPowerScaling || rightSideVectorWithPathing.getMagnitude() > maxPowerScaling) {
                     // too much power now, so we scale down the pathing vector
                     double pathingScalingFactor = Math.min(findNormalizingScaling(leftSideVector, pathingPower), findNormalizingScaling(rightSideVector, pathingPower));
                     truePathingVectors[0] = MathFunctions.addVectors(leftSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor));
@@ -113,7 +114,7 @@ public class DriveVectorScaler {
         wheelPowers[3] = (mecanumVectorsCopy[2].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[2].getYComponent()) / (mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent() - mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent());
 
         double wheelPowerMax = Math.max(Math.max(Math.abs(wheelPowers[0]), Math.abs(wheelPowers[1])), Math.max(Math.abs(wheelPowers[2]), Math.abs(wheelPowers[3])));
-        if (wheelPowerMax > 1) {
+        if (wheelPowerMax > maxPowerScaling) {
             wheelPowers[0] /= wheelPowerMax;
             wheelPowers[1] /= wheelPowerMax;
             wheelPowers[2] /= wheelPowerMax;
@@ -126,12 +127,12 @@ public class DriveVectorScaler {
     /**
      * This takes in two Vectors, one static and one variable, and returns the scaling factor that,
      * when multiplied to the variable Vector, results in magnitude of the sum of the static Vector
-     * and the scaled variable Vector being 1.
+     * and the scaled variable Vector being the max power scaling.
      *
      * IMPORTANT NOTE: I did not intend for this to be used for anything other than the method above
-     * this one in this class, so there will be errors if you input Vectors of length greater than 1,
+     * this one in this class, so there will be errors if you input Vectors of length greater than maxPowerScaling,
      * and it will scale up the variable Vector if the magnitude of the sum of the two input Vectors
-     * isn't greater than 1. So, just don't use this elsewhere. There's gotta be a better way to do
+     * isn't greater than maxPowerScaling. So, just don't use this elsewhere. There's gotta be a better way to do
      * whatever you're trying to do.
      *
      * I know that this is used outside of this class, however, I created this method so I get to
@@ -140,13 +141,32 @@ public class DriveVectorScaler {
      *
      * @param staticVector the Vector that is held constant.
      * @param variableVector the Vector getting scaled to make the sum of the input Vectors have a
-     *                       magnitude of 1.
+     *                       magnitude of maxPowerScaling.
      * @return returns the scaling factor for the variable Vector.
      */
     public double findNormalizingScaling(Vector staticVector, Vector variableVector) {
             double a = Math.pow(variableVector.getXComponent(), 2) + Math.pow(variableVector.getYComponent(), 2);
             double b = staticVector.getXComponent() * variableVector.getXComponent() + staticVector.getYComponent() * variableVector.getYComponent();
-            double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - 1.0;
+            double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - Math.pow(maxPowerScaling, 2);
             return (-b + Math.sqrt(Math.pow(b, 2) - a*c))/(a);
+
+    }
+
+    /**
+     * Sets the maximum power that can be used by the drive vector scaler. Clamped between 0 and 1.
+     *
+     * @param maxPowerScaling setting the max power scaling
+     */
+    public void setMaxPowerScaling(double maxPowerScaling) {
+        this.maxPowerScaling = MathFunctions.clamp(maxPowerScaling, 0, 1);
+    }
+
+    /**
+     * Gets the maximum power that can be used by the drive vector scaler. Ranges between 0 and 1.
+     *
+     * @return returns the max power scaling
+     */
+    public double getMaxPowerScaling() {
+        return maxPowerScaling;
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -1,20 +1,23 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.follower;
 
-import static org.firstinspires.ftc.teamcode.PedroConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID;
@@ -24,7 +27,6 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
@@ -94,7 +96,6 @@ public class Follower {
     private boolean holdPositionAtEnd;
     private boolean teleopDrive;
 
-    private double maxPower = 1;
     private double previousSecondaryTranslationalIntegral;
     private double previousTranslationalIntegral;
     private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling;
@@ -153,6 +154,16 @@ public class Follower {
         initialize();
     }
 
+    /**
+     * This creates a new Follower given a HardwareMap and a localizer.
+     * @param hardwareMap HardwareMap required
+     * @param localizer the localizer you wish to use
+     */
+//    public Follower(HardwareMap hardwareMap, Localizer localizer) {
+//        this.hardwareMap = hardwareMap;
+//        initialize(localizer);
+//    }
+
     /**
      * This initializes the follower.
      * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are
@@ -197,18 +208,7 @@ public class Follower {
      * @param set This caps the motor power from [0, 1].
      */
     public void setMaxPower(double set) {
-        maxPower = MathFunctions.clamp(set, 0, 1);
-    }
-
-    /**
-     * This handles the limiting of the drive powers array to the max power.
-     */
-    public void limitDrivePowers() {
-        for (int i = 0; i < drivePowers.length; i++) {
-            if (Math.abs(drivePowers[i]) > maxPower) {
-                drivePowers[i] = maxPower * MathFunctions.getSign(drivePowers[i]);
-            }
-        }
+        driveVectorScaler.setMaxPowerScaling(set);
     }
 
     /**
@@ -368,6 +368,25 @@ public class Follower {
         closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);
     }
 
+    /**
+     * This holds a Point.
+     *
+     * @param point   the Point to stay at.
+     * @param heading the heading to face.
+     */
+    public void holdPoint(Point point, double heading) {
+        holdPoint(new BezierPoint(point), heading);
+    }
+
+    /**
+     * This holds a Point.
+     *
+     * @param pose the Point (as a Pose) to stay at.
+     */
+    public void holdPoint(Pose pose) {
+        holdPoint(new Point(pose), pose.getHeading());
+    }
+
     /**
      * This follows a Path.
      * This also makes the Follower hold the last Point on the Path.
@@ -429,15 +448,22 @@ public class Follower {
     }
 
     /**
-     * This calls an update to the PoseUpdater, which updates the robot's current position estimate.
-     * This also updates all the Follower's PIDFs, which updates the motor powers.
+     * Calls an update to the PoseUpdater, which updates the robot's current position estimate.
      */
-    public void update() {
+    public void updatePose() {
         poseUpdater.update();
 
         if (drawOnDashboard) {
             dashboardPoseTracker.update();
         }
+    }
+
+    /**
+     * This calls an update to the PoseUpdater, which updates the robot's current position estimate.
+     * This also updates all the Follower's PIDFs, which updates the motor powers.
+     */
+    public void update() {
+        updatePose();
 
         if (!teleopDrive) {
             if (currentPath != null) {
@@ -446,8 +472,6 @@ public class Follower {
 
                     drivePowers = driveVectorScaler.getDrivePowers(MathFunctions.scalarMultiplyVector(getTranslationalCorrection(), holdPointTranslationalScaling), MathFunctions.scalarMultiplyVector(getHeadingVector(), holdPointHeadingScaling), new Vector(), poseUpdater.getPose().getHeading());
 
-                    limitDrivePowers();
-
                     for (int i = 0; i < motors.size(); i++) {
                         motors.get(i).setPower(drivePowers[i]);
                     }
@@ -459,8 +483,6 @@ public class Follower {
 
                         drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading());
 
-                        limitDrivePowers();
-
                         for (int i = 0; i < motors.size(); i++) {
                             motors.get(i).setPower(drivePowers[i]);
                         }
@@ -503,8 +525,6 @@ public class Follower {
 
             drivePowers = driveVectorScaler.getDrivePowers(getCentripetalForceCorrection(), teleopHeadingVector, teleopDriveVector, poseUpdater.getPose().getHeading());
 
-            limitDrivePowers();
-
             for (int i = 0; i < motors.size(); i++) {
                 motors.get(i).setPower(drivePowers[i]);
             }
@@ -518,7 +538,7 @@ public class Follower {
      *                     movement, this is the x-axis.
      * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric
      *                     movement, this is the y-axis.
-     * @param heading determines the heading vector for the robot in teleop.
+     * @param heading      determines the heading vector for the robot in teleop.
      */
     public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) {
         setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true);
@@ -531,7 +551,7 @@ public class Follower {
      *                     movement, this is the x-axis.
      * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric
      *                     movement, this is the y-axis.
-     * @param heading determines the heading vector for the robot in teleop.
+     * @param heading      determines the heading vector for the robot in teleop.
      * @param robotCentric sets if the movement will be field or robot centric
      */
     public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) {
@@ -670,19 +690,19 @@ public class Follower {
     public Vector getDriveVector() {
         if (!useDrive) return new Vector();
         if (followingPathChain && chainIndex < currentPathChain.size() - 1) {
-            return new Vector(1, currentPath.getClosestPointTangentVector().getTheta());
+            return new Vector(driveVectorScaler.getMaxPowerScaling(), currentPath.getClosestPointTangentVector().getTheta());
         }
 
         driveError = getDriveVelocityError();
 
         if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) {
             secondaryDrivePIDF.updateError(driveError);
-            driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
+            driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta());
             return MathFunctions.copyVector(driveVector);
         }
 
         drivePIDF.updateError(driveError);
-        driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
+        driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta());
         return MathFunctions.copyVector(driveVector);
     }
 
@@ -722,7 +742,7 @@ public class Follower {
         Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError);
 
         previousRawDriveError = rawDriveError;
-        rawDriveError =  velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
+        rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
 
         double projection = 2 * driveErrors[1] - driveErrors[0];
 
@@ -751,11 +771,11 @@ public class Follower {
         headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal());
         if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) {
             secondaryHeadingPIDF.updateError(headingError);
-            headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
+            headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading());
             return MathFunctions.copyVector(headingVector);
         }
         headingPIDF.updateError(headingError);
-        headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
+        headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading());
         return MathFunctions.copyVector(headingVector);
     }
 
@@ -772,7 +792,7 @@ public class Follower {
         Vector translational = getTranslationalCorrection();
         Vector corrective = MathFunctions.addVectors(centripetal, translational);
 
-        if (corrective.getMagnitude() > 1) {
+        if (corrective.getMagnitude() > driveVectorScaler.getMaxPowerScaling()) {
             return MathFunctions.addVectors(centripetal, MathFunctions.scalarMultiplyVector(translational, driveVectorScaler.findNormalizingScaling(centripetal, translational)));
         }
 
@@ -821,7 +841,7 @@ public class Follower {
             translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector);
         }
 
-        translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1));
+        translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, driveVectorScaler.getMaxPowerScaling()));
 
         this.translationalVector = MathFunctions.copyVector(translationalVector);
 
@@ -860,7 +880,7 @@ public class Follower {
             curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3));
         }
         if (Double.isNaN(curvature)) return new Vector();
-        centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
+        centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
         return centripetalVector;
     }
 
@@ -996,7 +1016,7 @@ public class Follower {
     /**
      * This resets the IMU, if applicable.
      */
-    public void resetIMU() {
+    private void resetIMU() throws InterruptedException {
         poseUpdater.resetIMU();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java

@@ -0,0 +1,520 @@
+
+/*   MIT License
+ *   Copyright (c) [2024] [Base 10 Assets, LLC]
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.pedroPathing.localization;
+
+import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt;
+
+import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
+import com.qualcomm.robotcore.util.TypeConversion;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.Arrays;
+
+
+@I2cDeviceType
+@DeviceProperties(
+        name = "goBILDA® Pinpoint Odometry Computer",
+        xmlTag = "goBILDAPinpoint",
+        description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)"
+)
+
+public class GoBildaPinpointDriver extends I2cDeviceSynchDevice<I2cDeviceSynchSimple> {
+
+    private int deviceStatus   = 0;
+    private int loopTime       = 0;
+    private int xEncoderValue  = 0;
+    private int yEncoderValue  = 0;
+    private float xPosition    = 0;
+    private float yPosition    = 0;
+    private float hOrientation = 0;
+    private float xVelocity    = 0;
+    private float yVelocity    = 0;
+    private float hVelocity    = 0;
+
+    private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod
+    private static final float goBILDA_4_BAR_POD    = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod
+
+    //i2c address of the device
+    public static final byte DEFAULT_ADDRESS = 0x31;
+
+    public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) {
+        super(deviceClient, deviceClientIsOwned);
+
+        this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS));
+        super.registerArmingStateCallback(false);
+    }
+
+
+    @Override
+    public Manufacturer getManufacturer() {
+        return Manufacturer.Other;
+    }
+
+    @Override
+    protected synchronized boolean doInitialize() {
+        ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
+        return true;
+    }
+
+    @Override
+    public String getDeviceName() {
+        return "goBILDA® Pinpoint Odometry Computer";
+    }
+
+
+    //Register map of the i2c device
+    private enum Register {
+        DEVICE_ID       (1),
+        DEVICE_VERSION  (2),
+        DEVICE_STATUS   (3),
+        DEVICE_CONTROL  (4),
+        LOOP_TIME       (5),
+        X_ENCODER_VALUE (6),
+        Y_ENCODER_VALUE (7),
+        X_POSITION      (8),
+        Y_POSITION      (9),
+        H_ORIENTATION   (10),
+        X_VELOCITY      (11),
+        Y_VELOCITY      (12),
+        H_VELOCITY      (13),
+        MM_PER_TICK     (14),
+        X_POD_OFFSET    (15),
+        Y_POD_OFFSET    (16),
+        YAW_SCALAR      (17),
+        BULK_READ       (18);
+
+        private final int bVal;
+
+        Register(int bVal){
+            this.bVal = bVal;
+        }
+    }
+
+    //Device Status enum that captures the current fault condition of the device
+    public enum DeviceStatus{
+        NOT_READY                (0),
+        READY                    (1),
+        CALIBRATING              (1 << 1),
+        FAULT_X_POD_NOT_DETECTED (1 << 2),
+        FAULT_Y_POD_NOT_DETECTED (1 << 3),
+        FAULT_NO_PODS_DETECTED   (1 << 2 | 1 << 3),
+        FAULT_IMU_RUNAWAY        (1 << 4);
+
+        private final int status;
+
+        DeviceStatus(int status){
+            this.status = status;
+        }
+    }
+
+    //enum that captures the direction the encoders are set to
+    public enum EncoderDirection{
+        FORWARD,
+        REVERSED;
+    }
+
+    //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used
+    public enum GoBildaOdometryPods {
+        goBILDA_SWINGARM_POD,
+        goBILDA_4_BAR_POD;
+    }
+    //enum that captures a limited scope of read data. More options may be added in future update
+    public enum readData {
+        ONLY_UPDATE_HEADING,
+    }
+
+
+    /** Writes an int to the i2c device
+     @param reg the register to write the int to
+     @param i the integer to write to the register
+     */
+    private void writeInt(final Register reg, int i){
+        deviceClient.write(reg.bVal, TypeConversion.intToByteArray(i,ByteOrder.LITTLE_ENDIAN));
+    }
+
+    /**
+     * Reads an int from a register of the i2c device
+     * @param reg the register to read from
+     * @return returns an int that contains the value stored in the read register
+     */
+    private int readInt(Register reg){
+        return byteArrayToInt(deviceClient.read(reg.bVal,4), ByteOrder.LITTLE_ENDIAN);
+    }
+
+    /**
+     * Converts a byte array to a float value
+     * @param byteArray byte array to transform
+     * @param byteOrder order of byte array to convert
+     * @return the float value stored by the byte array
+     */
+    private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){
+        return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat();
+    }
+    /**
+     * Reads a float from a register
+     * @param reg the register to read
+     * @return the float value stored in that register
+     */
+
+    private float readFloat(Register reg){
+        return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN);
+    }
+
+
+    /**
+     * Converts a float to a byte array
+     * @param value the float array to convert
+     * @return the byte array converted from the float
+     */
+    private byte [] floatToByteArray (float value, ByteOrder byteOrder) {
+        return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array();
+    }
+
+    /**
+     * Writes a byte array to a register on the i2c device
+     * @param reg the register to write to
+     * @param bytes the byte array to write
+     */
+    private void writeByteArray (Register reg, byte[] bytes){
+        deviceClient.write(reg.bVal,bytes);
+    }
+
+    /**
+     * Writes a float to a register on the i2c device
+     * @param reg the register to write to
+     * @param f the float to write
+     */
+    private void writeFloat (Register reg, float f){
+        byte[] bytes = ByteBuffer.allocate(4).order(ByteOrder.LITTLE_ENDIAN).putFloat(f).array();
+        deviceClient.write(reg.bVal,bytes);
+    }
+
+    /**
+     * Looks up the DeviceStatus enum corresponding with an int value
+     * @param s int to lookup
+     * @return the Odometry Computer state
+     */
+    private DeviceStatus lookupStatus (int s){
+        if ((s & DeviceStatus.CALIBRATING.status) != 0){
+            return DeviceStatus.CALIBRATING;
+        }
+        boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0;
+        boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0;
+
+        if(!xPodDetected  && !yPodDetected){
+            return DeviceStatus.FAULT_NO_PODS_DETECTED;
+        }
+        if (!xPodDetected){
+            return DeviceStatus.FAULT_X_POD_NOT_DETECTED;
+        }
+        if (!yPodDetected){
+            return DeviceStatus.FAULT_Y_POD_NOT_DETECTED;
+        }
+        if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){
+            return DeviceStatus.FAULT_IMU_RUNAWAY;
+        }
+        if ((s & DeviceStatus.READY.status) != 0){
+            return DeviceStatus.READY;
+        }
+        else {
+            return DeviceStatus.NOT_READY;
+        }
+    }
+
+    /**
+     * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called.
+     */
+    public void update(){
+        byte[] bArr   = deviceClient.read(Register.BULK_READ.bVal, 40);
+        deviceStatus  = byteArrayToInt(Arrays.copyOfRange  (bArr, 0, 4),  ByteOrder.LITTLE_ENDIAN);
+        loopTime      = byteArrayToInt(Arrays.copyOfRange  (bArr, 4, 8),  ByteOrder.LITTLE_ENDIAN);
+        xEncoderValue = byteArrayToInt(Arrays.copyOfRange  (bArr, 8, 12), ByteOrder.LITTLE_ENDIAN);
+        yEncoderValue = byteArrayToInt(Arrays.copyOfRange  (bArr, 12,16), ByteOrder.LITTLE_ENDIAN);
+        xPosition     = byteArrayToFloat(Arrays.copyOfRange(bArr, 16,20), ByteOrder.LITTLE_ENDIAN);
+        yPosition     = byteArrayToFloat(Arrays.copyOfRange(bArr, 20,24), ByteOrder.LITTLE_ENDIAN);
+        hOrientation  = byteArrayToFloat(Arrays.copyOfRange(bArr, 24,28), ByteOrder.LITTLE_ENDIAN);
+        xVelocity     = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN);
+        yVelocity     = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN);
+        hVelocity     = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN);
+    }
+
+    /**
+     * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function
+     * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING
+     * is supported.
+     * @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING
+     */
+    public void update(readData data) {
+        if (data == readData.ONLY_UPDATE_HEADING) {
+            hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN);
+        }
+    }
+
+    /**
+     * Sets the odometry pod positions relative to the point that the odometry computer tracks around.<br><br>
+     * The most common tracking position is the center of the robot. <br> <br>
+     * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number. <br>
+     * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.<br>
+     * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases
+     * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases
+     */
+    public void setOffsets(double xOffset, double yOffset){
+        writeFloat(Register.X_POD_OFFSET, (float) xOffset);
+        writeFloat(Register.Y_POD_OFFSET, (float) yOffset);
+    }
+
+    /**
+     * Recalibrates the Odometry Computer's internal IMU. <br><br>
+     * <strong> Robot MUST be stationary </strong> <br><br>
+     * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds.
+     */
+    public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);}
+
+    /**
+     * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU. <br><br>
+     * <strong> Robot MUST be stationary </strong> <br><br>
+     * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds.
+     */
+    public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);}
+
+    /**
+     * Can reverse the direction of each encoder.
+     * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward
+     * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left
+     */
+    public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){
+        if (xEncoder == EncoderDirection.FORWARD){
+            writeInt(Register.DEVICE_CONTROL,1<<5);
+        }
+        if (xEncoder == EncoderDirection.REVERSED) {
+            writeInt(Register.DEVICE_CONTROL,1<<4);
+        }
+
+        if (yEncoder == EncoderDirection.FORWARD){
+            writeInt(Register.DEVICE_CONTROL,1<<3);
+        }
+        if (yEncoder == EncoderDirection.REVERSED){
+            writeInt(Register.DEVICE_CONTROL,1<<2);
+        }
+    }
+
+    /**
+     * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.<br><br>
+     * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD
+     */
+    public void setEncoderResolution(GoBildaOdometryPods pods){
+        if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) {
+            writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN)));
+        }
+        if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){
+            writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN)));
+        }
+    }
+
+    /**
+     * Sets the encoder resolution in ticks per mm of the odometry pods. <br>
+     * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel.
+     * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192
+     */
+    public void setEncoderResolution(double ticks_per_mm){
+        writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN)));
+    }
+
+    /**
+     * Tuning this value should be unnecessary.<br>
+     * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.<br><br>
+     * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures. <br><br>
+     * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured.
+     * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.<br><br>
+     * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com
+     * @param yawOffset A scalar for the robot's heading.
+     */
+    public void setYawScalar(double yawOffset){
+        writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN)));
+    }
+
+    /**
+     * Send a position that the Pinpoint should use to track your robot relative to. You can use this to
+     * update the estimated position of your robot with new external sensor data, or to run a robot
+     * in field coordinates. <br><br>
+     * This overrides the current position. <br><br>
+     * <strong>Using this feature to track your robot's position in field coordinates:</strong> <br>
+     * When you start your code, send a Pose2D that describes the starting position on the field of your robot. <br>
+     * Say you're on the red alliance, your robot is against the wall and closer to the audience side,
+     * and the front of your robot is pointing towards the center of the field.
+     * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always
+     * keep track of how far away from the center of the field you are. <br><br>
+     * <strong>Using this feature to update your position with additional sensors: </strong><br>
+     * Some robots have a secondary way to locate their robot on the field. This is commonly
+     * Apriltag localization in FTC, but it can also be something like a distance sensor.
+     * Often these external sensors are absolute (meaning they measure something about the field)
+     * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific
+     * position on the field to use them. In that case, spend most of your time relying on the Pinpoint
+     * to determine your location. Then when you pull a new position from your secondary sensor,
+     * send a setPosition command with the new position. The Pinpoint will then track your movement
+     * relative to that new, more accurate position.
+     * @param pos a Pose2D describing the robot's new position.
+     */
+    public Pose2D setPosition(Pose2D pos){
+        writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN)));
+        writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN)));
+        writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN)));
+        return pos;
+    }
+
+    /**
+     * Checks the deviceID of the Odometry Computer. Should return 1.
+     * @return 1 if device is functional.
+     */
+    public int getDeviceID(){return readInt(Register.DEVICE_ID);}
+
+    /**
+     * @return the firmware version of the Odometry Computer
+     */
+    public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); }
+
+    public float getYawScalar(){return readFloat(Register.YAW_SCALAR); }
+
+    /**
+     * Device Status stores any faults the Odometry Computer may be experiencing. These faults include:
+     * @return one of the following states:<br>
+     * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED<br>
+     * READY - The device is currently functioning as normal. GREEN LED<br>
+     * CALIBRATING - The device is currently recalibrating the gyro. RED LED<br>
+     * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED <br>
+     * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED <br>
+     * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED <br>
+     */
+    public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); }
+
+    /**
+     * Checks the Odometry Computer's most recent loop time.<br><br>
+     * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com
+     * @return loop time in microseconds (1/1,000,000 seconds)
+     */
+    public int getLoopTime(){return loopTime; }
+
+    /**
+     * Checks the Odometry Computer's most recent loop frequency.<br><br>
+     * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com
+     * @return Pinpoint Frequency in Hz (loops per second),
+     */
+    public double getFrequency(){
+        if (loopTime != 0){
+            return 1000000.0/loopTime;
+        }
+        else {
+            return 0;
+        }
+    }
+
+    /**
+     * @return the raw value of the X (forward) encoder in ticks
+     */
+    public int getEncoderX(){return xEncoderValue; }
+
+    /**
+     * @return the raw value of the Y (strafe) encoder in ticks
+     */
+    public int getEncoderY(){return yEncoderValue; }
+
+    /**
+     * @return the estimated X (forward) position of the robot in mm
+     */
+    public double getPosX(){return xPosition; }
+
+    /**
+     * @return the estimated Y (Strafe) position of the robot in mm
+     */
+    public double getPosY(){return yPosition; }
+
+    /**
+     * @return the estimated H (heading) position of the robot in Radians
+     */
+    public double getHeading(){return hOrientation;}
+
+    /**
+     * @return the estimated X (forward) velocity of the robot in mm/sec
+     */
+    public double getVelX(){return xVelocity; }
+
+    /**
+     * @return the estimated Y (strafe) velocity of the robot in mm/sec
+     */
+    public double getVelY(){return yVelocity; }
+
+    /**
+     * @return the estimated H (heading) velocity of the robot in radians/sec
+     */
+    public double getHeadingVelocity(){return hVelocity; }
+
+    /**
+     * <strong> This uses its own I2C read, avoid calling this every loop. </strong>
+     * @return the user-set offset for the X (forward) pod
+     */
+    public float getXOffset(){return readFloat(Register.X_POD_OFFSET);}
+
+    /**
+     * <strong> This uses its own I2C read, avoid calling this every loop. </strong>
+     * @return the user-set offset for the Y (strafe) pod
+     */
+    public float getYOffset(){return readFloat(Register.Y_POD_OFFSET);}
+
+    /**
+     * @return a Pose2D containing the estimated position of the robot
+     */
+    public Pose2D getPosition(){
+        return new Pose2D(DistanceUnit.MM,
+                xPosition,
+                yPosition,
+                AngleUnit.RADIANS,
+                hOrientation);
+    }
+
+
+
+    /**
+     * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second
+     */
+    public Pose2D getVelocity(){
+        return new Pose2D(DistanceUnit.MM,
+                xVelocity,
+                yVelocity,
+                AngleUnit.RADIANS,
+                hVelocity);
+    }
+
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization;
 
+import com.qualcomm.robotcore.hardware.IMU;
+
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 
 /**
@@ -90,5 +92,14 @@ public abstract class Localizer {
     /**
      * This resets the IMU of the localizer, if applicable.
      */
-    public abstract void resetIMU();
+    public abstract void resetIMU() throws InterruptedException;
+
+    /**
+     * This is overridden to return the IMU, if there is one.
+     *
+     * @return returns the IMU if it exists
+     */
+    public IMU getIMU() {
+        return null;
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization;
 
+import androidx.annotation.NonNull;
+
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 
@@ -44,7 +46,7 @@ public class Pose {
      * This creates a new Pose with no inputs and 0 for all values.
      */
     public Pose() {
-        this(0,0,0);
+        this(0, 0, 0);
     }
 
     /**
@@ -208,4 +210,10 @@ public class Pose {
     public Pose copy() {
         return new Pose(getX(), getY(), getHeading());
     }
+
+    @NonNull
+    @Override
+    public String toString() {
+        return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")";
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java

@@ -60,6 +60,7 @@ public class PoseUpdater {
         }
 
         this.localizer = localizer;
+        imu = localizer.getIMU();
     }
 
     /**
@@ -261,9 +262,10 @@ public class PoseUpdater {
      */
     public Vector getVelocity() {
         if (currentVelocity == null) {
-            currentVelocity = new Vector();
-            currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY());
-            currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9)));
+//            currentVelocity = new Vector();
+//            currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY());
+//            currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9)));
+            currentVelocity = localizer.getVelocityVector();
             return MathFunctions.copyVector(currentVelocity);
         } else {
             return MathFunctions.copyVector(currentVelocity);
@@ -300,7 +302,9 @@ public class PoseUpdater {
      * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset.
      */
     public void resetHeadingToIMU() {
-        localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
+        if (imu != null) {
+            localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
+        }
     }
 
     /**
@@ -309,7 +313,9 @@ public class PoseUpdater {
      * method.
      */
     public void resetHeadingToIMUWithOffsets() {
-        setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
+        if (imu != null) {
+            setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
+        }
     }
 
     /**
@@ -318,7 +324,10 @@ public class PoseUpdater {
      * @return returns the normalized IMU heading.
      */
     public double getNormalizedIMUHeading() {
-        return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
+        if (imu != null) {
+            return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
+        }
+        return 0;
     }
 
     /**
@@ -342,7 +351,7 @@ public class PoseUpdater {
     /**
      *
      */
-    public void resetIMU() {
+    public void resetIMU() throws InterruptedException {
         localizer.resetIMU();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java

@@ -71,8 +71,8 @@ public class DriveEncoderLocalizer extends Localizer {
 
         // TODO: reverse any encoders necessary
         leftFront.setDirection(Encoder.REVERSE);
-        rightRear.setDirection(Encoder.REVERSE);
-        leftRear.setDirection(Encoder.FORWARD);
+        leftRear.setDirection(Encoder.REVERSE);
+        rightFront.setDirection(Encoder.FORWARD);
         rightRear.setDirection(Encoder.FORWARD);
 
         setStartPose(setStartPose);
@@ -182,7 +182,7 @@ public class DriveEncoderLocalizer extends Localizer {
         globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
 
         displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
-        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
+        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9)));
 
         totalHeading += globalDeltas.get(2, 0);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java

@@ -21,18 +21,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
  *
  * forward on robot is the x positive direction
  *
- *    /--------------\
- *    |     ____     |
- *    |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
- *    \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
+ *                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
+ *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         | ||        || |
+ *  left (y positive) <--- | ||        || |  
+ *                         |     ____     |
+ *                         |     ----     |
+ *                         \--------------/
  *
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 7/20/2024
@@ -41,6 +41,9 @@ public class OTOSLocalizer extends Localizer {
     private HardwareMap hardwareMap;
     private Pose startPose;
     private SparkFunOTOS otos;
+    private SparkFunOTOS.Pose2D otosPose;
+    private SparkFunOTOS.Pose2D otosVel;
+    private SparkFunOTOS.Pose2D otosAcc;
     private double previousHeading;
     private double totalHeading;
 
@@ -64,13 +67,12 @@ public class OTOSLocalizer extends Localizer {
     public OTOSLocalizer(HardwareMap map, Pose setStartPose) {
         hardwareMap = map;
 
-        // TODO: replace this with your OTOS port
         /*
          TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the
           'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a
-          "SparkFunOTOS Corrected" in your robot confg
+          "SparkFunOTOS Corrected" in your robot config
          */
-         SparkFunOTOS
+        // TODO: replace this with your OTOS port
         otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
 
         otos.setLinearUnit(DistanceUnit.INCH);
@@ -90,6 +92,9 @@ public class OTOSLocalizer extends Localizer {
         otos.resetTracking();
 
         setStartPose(setStartPose);
+        otosPose = new SparkFunOTOS.Pose2D();
+        otosVel = new SparkFunOTOS.Pose2D();
+        otosAcc = new SparkFunOTOS.Pose2D();
         totalHeading = 0;
         previousHeading = startPose.getHeading();
 
@@ -103,8 +108,12 @@ public class OTOSLocalizer extends Localizer {
      */
     @Override
     public Pose getPose() {
-        SparkFunOTOS.Pose2D pose = otos.getPosition();
-        return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h));
+        Pose pose = new Pose(otosPose.x, otosPose.y, otosPose.h);
+
+        Vector vec = pose.getVector();
+        vec.rotateVector(startPose.getHeading());
+
+        return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
     }
 
     /**
@@ -114,8 +123,7 @@ public class OTOSLocalizer extends Localizer {
      */
     @Override
     public Pose getVelocity() {
-        SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity();
-        return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h);
+        return new Pose(otosVel.x, otosVel.y, otosVel.h);
     }
 
     /**
@@ -157,8 +165,9 @@ public class OTOSLocalizer extends Localizer {
      */
     @Override
     public void update() {
-        totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading);
-        previousHeading = otos.getPosition().h;
+        otos.getPosVelAcc(otosPose,otosVel,otosAcc);
+        totalHeading += MathFunctions.getSmallestAngleDifference(otosPose.h, previousHeading);
+        previousHeading = otosPose.h;
     }
 
     /**
@@ -215,4 +224,4 @@ public class OTOSLocalizer extends Localizer {
      */
     public void resetIMU() {
     }
-}
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java

@@ -0,0 +1,245 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
+
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
+import org.opencv.core.Mat;
+
+/**
+ * This is the Pinpoint class. This class extends the Localizer superclass and is a
+ * localizer that uses the two wheel odometry set up with the IMU to have more accurate heading
+ * readings. The diagram below, which is modified from Road Runner, shows a typical set up.
+ *
+ * The view is from the top of the robot looking downwards.
+ *
+ * left on robot is the y positive direction
+ *
+ * forward on robot is the x positive direction
+ *
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |
+ *    | ||           |
+ *    | ||           |  ----> left (y positive)
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *           |
+ *           |
+ *           V
+ *    forward (x positive)
+ * With the pinpoint your readings will be used in mm
+ * to use inches ensure to divide your mm value by 25.4
+ * @author Logan Nash
+ * @author Havish Sripada 12808 - RevAmped Robotics
+ * @author Ethan Doak - Gobilda
+ * @version 1.0, 10/2/2024
+ */
+public class PinpointLocalizer extends Localizer {
+    private HardwareMap hardwareMap;
+    private GoBildaPinpointDriver odo;
+    private double previousHeading;
+    private double totalHeading;
+    private Pose startPose;
+    private long deltaTimeNano;
+    private NanoTimer timer;
+    private Pose currentVelocity;
+    private Pose previousPinpointPose;
+
+    /**
+     * This creates a new PinpointLocalizer from a HardwareMap, with a starting Pose at (0,0)
+     * facing 0 heading.
+     *
+     * @param map the HardwareMap
+     */
+    public PinpointLocalizer(HardwareMap map){ this(map, new Pose());}
+
+    /**
+     * This creates a new PinpointLocalizer from a HardwareMap and a Pose, with the Pose
+     * specifying the starting pose of the localizer.
+     *
+     * @param map the HardwareMap
+     * @param setStartPose the Pose to start from
+     */
+    public PinpointLocalizer(HardwareMap map, Pose setStartPose){
+        hardwareMap = map;
+
+        odo = hardwareMap.get(GoBildaPinpointDriver.class,"pinpoint");
+
+        //The default units are inches, but you can swap the units if you wish.
+        //If you have already tuned the TwoWheelLocalizer, you can simply use the forwardEncoderPose's y value and strafeEncoderPose's x values.
+        setOffsets(-2.815, 0.125, DistanceUnit.INCH); //these are tuned for 3110-0002-0001 Product Insight #1
+
+        //TODO: Tune urself if needed
+//        odo.setYawScalar(1.0);
+
+        odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
+        //odo.setEncoderResolution(13.26291192);
+
+        odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.REVERSED, GoBildaPinpointDriver.EncoderDirection.FORWARD);
+
+        resetPinpoint();
+
+        setStartPose(setStartPose);
+        totalHeading = 0;
+        timer = new NanoTimer();
+        previousPinpointPose = new Pose();
+        currentVelocity = new Pose();
+        deltaTimeNano = 1;
+        previousHeading = setStartPose.getHeading();
+    }
+
+    /**
+     * This returns the current pose estimate.
+     *
+     * @return returns the current pose estimate as a Pose
+     */
+    @Override
+    public Pose getPose() {
+        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(previousPinpointPose, startPose.getHeading(), false));
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Pose
+     */
+    @Override
+    public Pose getVelocity() {
+        return currentVelocity.copy();
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Vector
+     */
+    @Override
+    public Vector getVelocityVector() {
+        return currentVelocity.getVector();
+    }
+
+    /**
+     * This sets the start pose. Since nobody should be using this after the robot has begun moving,
+     * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose).
+     *
+     * @param setStart the new start pose
+     */
+    @Override
+    public void setStartPose(Pose setStart) {
+        this.startPose = setStart;
+    }
+
+    /**
+     * This sets the current pose estimate. Changing this should just change the robot's current
+     * pose estimate, not anything to do with the start pose.
+     *
+     * @param setPose the new current pose estimate
+     */
+    @Override
+    public void setPose(Pose setPose) {
+        Pose setNewPose = MathFunctions.subtractPoses(setPose, startPose);
+        odo.setPosition(new Pose2D(DistanceUnit.INCH, setNewPose.getX(), setNewPose.getY(), AngleUnit.RADIANS, setNewPose.getHeading()));
+    }
+
+    /**
+     * This updates the total heading of the robot. The Pinpoint handles all other updates itself.
+     */
+    @Override
+    public void update() {
+        deltaTimeNano = timer.getElapsedTime();
+        timer.resetTimer();
+        odo.update();
+        Pose2D pinpointPose = odo.getPosition();
+        Pose currentPinpointPose = new Pose(pinpointPose.getX(DistanceUnit.INCH), pinpointPose.getY(DistanceUnit.INCH), pinpointPose.getHeading(AngleUnit.RADIANS));
+        totalHeading += MathFunctions.getSmallestAngleDifference(currentPinpointPose.getHeading(), previousHeading);
+        previousHeading = currentPinpointPose.getHeading();
+        Pose deltaPose = MathFunctions.subtractPoses(currentPinpointPose, previousPinpointPose);
+        currentVelocity = new Pose(deltaPose.getX() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getY() / (deltaTimeNano / Math.pow(10.0, 9)), deltaPose.getHeading() / (deltaTimeNano / Math.pow(10.0, 9)));
+        previousPinpointPose = currentPinpointPose;
+    }
+
+    /**
+     * This returns how far the robot has turned in radians, in a number not clamped between 0 and
+     * 2 * pi radians. This is used for some tuning things and nothing actually within the following.
+     *
+     * @return returns how far the robot has turned in total, in radians.
+     */
+    @Override
+    public double getTotalHeading() {
+        return totalHeading;
+    }
+
+    /**
+     * This returns the Y encoder value as none of the odometry tuners are required for this localizer
+     * @return returns the Y encoder value
+     */
+    @Override
+    public double getForwardMultiplier() {
+        return odo.getEncoderY();
+    }
+
+    /**
+     * This returns the X encoder value as none of the odometry tuners are required for this localizer
+     * @return returns the X encoder value
+     */
+    @Override
+    public double getLateralMultiplier() {
+        return odo.getEncoderX();
+    }
+
+    /**
+     * This returns either the factory tuned yaw scalar or the yaw scalar tuned by yourself.
+     * @return returns the yaw scalar
+     */
+    @Override
+    public double getTurningMultiplier() {
+        return odo.getYawScalar();
+    }
+
+    /**
+     * This sets the offsets and converts inches to millimeters
+     * @param xOffset How far to the side from the center of the robot is the x-pod? Use positive values if it's to the left and negative if it's to the right.
+     * @param yOffset How far forward from the center of the robot is the y-pod? Use positive values if it's forward and negative if it's to the back.
+     * @param unit The units that the measurements are given in
+     */
+    private void setOffsets(double xOffset, double yOffset, DistanceUnit unit) {
+        odo.setOffsets(unit.toMm(xOffset), unit.toMm(yOffset));
+    }
+
+    /**
+     * This resets the IMU. Does not change heading estimation.
+     */
+    @Override
+    public void resetIMU() throws InterruptedException {
+        odo.recalibrateIMU();
+
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+
+    /**
+     * This resets the pinpoint.
+     */
+    private void resetPinpoint() {
+        odo.resetPosAndIMU();
+
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java

@@ -1,7 +1,5 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedroConstants.*;
-
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -28,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
  *
  * forward on robot is the x positive direction
  *
- *    /--------------\
- *    |     ____     |
- *    |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
- *    \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
+ *                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
+ *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         | ||        || |
+ *  left (y positive) <--- | ||        || |  
+ *                         |     ____     |
+ *                         |     ----     |
+ *                         \--------------/
  *
  * @author Logan Nash
  * @author Anyi Lin - 10158 Scott's Bots
@@ -65,9 +63,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
     private double previousIMUOrientation;
     private double deltaRadians;
     private double totalHeading;
-    public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
-    public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
-    public static double TURN_TICKS_TO_RADIANS = 0.0022;//8192 * 1.37795 * 2 * Math.PI * 0.5;
+    public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
+    public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
+    public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5;
 
     public static boolean useIMU = true;
 
@@ -90,25 +88,26 @@ public class ThreeWheelIMULocalizer extends Localizer {
      */
     public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) {
         hardwareMap = map;
-        imu = hardwareMap.get(IMU.class, IMU);
+        imu = hardwareMap.get(IMU.class, "imu");
 
         // TODO: replace this with your IMU's orientation
-        imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION)));
+        imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP)));
 
         // TODO: replace these with your encoder positions
-        leftEncoderPose = new Pose(0, 6.19375, 0);
-        rightEncoderPose = new Pose(0, -6.19375, 0);
-        strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
+        leftEncoderPose = new Pose(-3, 5.7, 0);
+        rightEncoderPose = new Pose(-3, -5.7, 0);
+        strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90));
+
 
         // TODO: replace these with your encoder ports
-        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
-        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
-        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
+        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lb"));
+        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lf"));
+        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rf"));
 
         // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
-        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
-        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
+        leftEncoder.setDirection(Encoder.REVERSE);
+        rightEncoder.setDirection(Encoder.FORWARD);
+        strafeEncoder.setDirection(Encoder.FORWARD);
 
         setStartPose(setStartPose);
         timer = new NanoTimer();
@@ -220,7 +219,7 @@ public class ThreeWheelIMULocalizer extends Localizer {
         globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
 
         displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
-        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
+        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9)));
 
         totalHeading += globalDeltas.get(2, 0);
     }
@@ -314,4 +313,14 @@ public class ThreeWheelIMULocalizer extends Localizer {
     public void resetIMU() {
         imu.resetYaw();
     }
-}
+
+    /**
+     * This is returns the IMU.
+     *
+     * @return returns the IMU
+     */
+    @Override
+    public IMU getIMU() {
+        return imu;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java

@@ -25,18 +25,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
  *
  * forward on robot is the x positive direction
  *
- *    /--------------\
- *    |     ____     |
- *    |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
- *    \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
+ *                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
+ *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         | ||        || |
+ *  left (y positive) <--- | ||        || |
+ *                         |     ____     |
+ *                         |     ----     |
+ *                         \--------------/
  *
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 4/2/2024
@@ -57,9 +57,9 @@ public class ThreeWheelLocalizer extends Localizer {
     private Pose rightEncoderPose;
     private Pose strafeEncoderPose;
     private double totalHeading;
-    public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
-    public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
-    public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
+    public static double FORWARD_TICKS_TO_INCHES = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
+    public static double STRAFE_TICKS_TO_INCHES = -0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
+    public static double TURN_TICKS_TO_RADIANS = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5;
 
     /**
      * This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
@@ -80,9 +80,9 @@ public class ThreeWheelLocalizer extends Localizer {
      */
     public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
         // TODO: replace these with your encoder positions
-        leftEncoderPose = new Pose(0, 6.19375, 0);
-        rightEncoderPose = new Pose(0, -6.19375, 0);
-        strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
+        leftEncoderPose = new Pose(0.25, 6.25, 0);
+        rightEncoderPose = new Pose(0.25, -6.25, 0);
+        strafeEncoderPose = new Pose(-7, 0.25, Math.toRadians(90));
 
         hardwareMap = map;
 
@@ -92,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer {
         strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
 
         // TODO: reverse any encoders necessary
-        //leftEncoder.setDirection(Encoder.REVERSE);
-       // rightEncoder.setDirection(Encoder.REVERSE);
-        //strafeEncoder.setDirection(Encoder.FORWARD);
+        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
+        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
+        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
 
         setStartPose(setStartPose);
         timer = new NanoTimer();
@@ -206,7 +206,7 @@ public class ThreeWheelLocalizer extends Localizer {
         globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
 
         displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
-        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
+        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9)));
 
         totalHeading += globalDeltas.get(2, 0);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java

@@ -1,8 +1,5 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
-
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -29,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
  *
  * forward on robot is the x positive direction
  *
- *    /--------------\
- *    |     ____     |
- *    |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
- *    \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
+*                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
+ *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         |           || |
+ *  left (y positive) <--- |           || |  
+ *                         |     ____     |
+ *                         |     ----     |
+ *                         \--------------/
  *
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 4/2/2024
@@ -94,8 +91,8 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w
         imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT)));
 
         // TODO: replace these with your encoder ports
-        forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
-        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
+        forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
+        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
 
         // TODO: reverse any encoders necessary
         forwardEncoder.setDirection(Encoder.REVERSE);
@@ -211,7 +208,7 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w
         globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
 
         displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
-        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
+        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9)));
 
         totalHeading += globalDeltas.get(2, 0);
     }
@@ -299,4 +296,14 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w
     public void resetIMU() {
         imu.resetYaw();
     }
+
+    /**
+     * This is returns the IMU.
+     *
+     * @return returns the IMU
+     */
+    @Override
+    public IMU getIMU() {
+        return imu;
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java

@@ -0,0 +1,321 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
+
+
+/**
+ * This is the TwoWheelPinpointIMULocalizer class. This class extends the Localizer superclass and is a
+ * localizer that uses the two wheel odometry with the Pinpoint IMU set up. The diagram below, which is modified from
+ * Road Runner, shows a typical set up.
+ *
+ * The view is from the top of the robot looking downwards.
+ *
+ * left on robot is the y positive direction
+ *
+ * forward on robot is the x positive direction
+ *
+*                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
+ *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         |           || |
+ *  left (y positive) <--- |           || |  
+ *                         |     ____     |
+ *                         |     ----     |
+ *                         \--------------/
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Havish Sripada - 12808 RevAmped Robotics
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 11/28/2024
+ */
+@Config
+public class TwoWheelPinpointIMULocalizer extends Localizer {
+    private HardwareMap hardwareMap;
+    private GoBildaPinpointDriver pinpoint;
+    private Pose startPose;
+    private Pose displacementPose;
+    private Pose currentVelocity;
+    private Matrix prevRotationMatrix;
+    private NanoTimer timer;
+    private long deltaTimeNano;
+    private Encoder forwardEncoder;
+    private Encoder strafeEncoder;
+    private Pose forwardEncoderPose;
+    private Pose strafeEncoderPose;
+    private double previousHeading;
+    private double deltaRadians;
+    private double totalHeading;
+    public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
+    public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
+
+    /**
+     * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
+     * facing 0 heading.
+     *
+     * @param map the HardwareMap
+     */
+    public TwoWheelPinpointIMULocalizer(HardwareMap map) {
+        this(map, new Pose());
+    }
+
+    /**
+     * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose
+     * specifying the starting pose of the localizer.
+     *
+     * @param map          the HardwareMap
+     * @param setStartPose the Pose to start from
+     */
+    public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) {
+        // TODO: replace these with your encoder positions
+        forwardEncoderPose = new Pose(-0.82, 6.47, 0);
+        strafeEncoderPose = new Pose(-4, -0.273, Math.toRadians(90));
+
+        hardwareMap = map;
+
+        pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
+        pinpoint.resetPosAndIMU();
+
+        // TODO: replace these with your encoder ports
+        forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
+        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
+
+        // TODO: reverse any encoders necessary
+        forwardEncoder.setDirection(Encoder.REVERSE);
+        strafeEncoder.setDirection(Encoder.FORWARD);
+
+        setStartPose(setStartPose);
+        timer = new NanoTimer();
+        deltaTimeNano = 1;
+        displacementPose = new Pose();
+        currentVelocity = new Pose();
+        previousHeading = startPose.getHeading();
+        deltaRadians = 0;
+    }
+
+    /**
+     * This returns the current pose estimate.
+     *
+     * @return returns the current pose estimate as a Pose
+     */
+    @Override
+    public Pose getPose() {
+        return new Pose(startPose.getX()+displacementPose.getX(), startPose.getY()+displacementPose.getY(),displacementPose.getHeading());
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Pose
+     */
+    @Override
+    public Pose getVelocity() {
+        return currentVelocity.copy();
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Vector
+     */
+    @Override
+    public Vector getVelocityVector() {
+        return currentVelocity.getVector();
+    }
+
+    /**
+     * This sets the start pose. Changing the start pose should move the robot as if all its
+     * previous movements were displacing it from its new start pose.
+     *
+     * @param setStart the new start pose
+     */
+    @Override
+    public void setStartPose(Pose setStart) {
+        startPose = setStart;
+    }
+
+    /**
+     * This sets the Matrix that contains the previous pose's heading rotation.
+     *
+     * @param heading the rotation of the Matrix
+     */
+    public void setPrevRotationMatrix(double heading) {
+        prevRotationMatrix = new Matrix(3, 3);
+        prevRotationMatrix.set(0, 0, Math.cos(heading));
+        prevRotationMatrix.set(0, 1, -Math.sin(heading));
+        prevRotationMatrix.set(1, 0, Math.sin(heading));
+        prevRotationMatrix.set(1, 1, Math.cos(heading));
+        prevRotationMatrix.set(2, 2, 1.0);
+    }
+
+    /**
+     * This sets the current pose estimate. Changing this should just change the robot's current
+     * pose estimate, not anything to do with the start pose.
+     *
+     * @param setPose the new current pose estimate
+     */
+    @Override
+    public void setPose(Pose setPose) {
+        displacementPose = MathFunctions.subtractPoses(setPose, startPose);
+        resetEncoders();
+    }
+
+    /**
+     * This updates the elapsed time timer that keeps track of time between updates, as well as the
+     * change position of the Encoders and the IMU readings. Then, the robot's global change in
+     * position is calculated using the pose exponential method.
+     */
+    @Override
+    public void update() {
+        deltaTimeNano = timer.getElapsedTime();
+        timer.resetTimer();
+
+        updateEncoders();
+        Matrix robotDeltas = getRobotDeltas();
+        Matrix globalDeltas;
+        setPrevRotationMatrix(getPose().getHeading());
+
+        Matrix transformation = new Matrix(3, 3);
+        if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
+            transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
+            transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
+            transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
+            transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
+            transformation.set(2, 2, 1.0);
+        } else {
+            transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
+            transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
+            transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
+            transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
+            transformation.set(2, 2, 1.0);
+        }
+
+        globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
+
+        displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
+        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9)));
+
+        totalHeading += globalDeltas.get(2, 0);
+    }
+
+    /**
+     * This updates the Encoders as well as the IMU.
+     */
+    public void updateEncoders() {
+        forwardEncoder.update();
+        strafeEncoder.update();
+
+        pinpoint.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING);
+        double currentHeading = startPose.getHeading() + MathFunctions.normalizeAngle(pinpoint.getHeading());
+        deltaRadians = MathFunctions.getTurnDirection(previousHeading, currentHeading) * MathFunctions.getSmallestAngleDifference(currentHeading, previousHeading);
+        previousHeading = currentHeading;
+    }
+
+    /**
+     * This resets the Encoders.
+     */
+    public void resetEncoders() {
+        forwardEncoder.reset();
+        strafeEncoder.reset();
+    }
+
+    /**
+     * This calculates the change in position from the perspective of the robot using information
+     * from the Encoders and IMU.
+     *
+     * @return returns a Matrix containing the robot relative movement.
+     */
+    public Matrix getRobotDeltas() {
+        Matrix returnMatrix = new Matrix(3, 1);
+        // x/forward movement
+        returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians));
+        //y/strafe movement
+        returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians));
+        // theta/turning
+        returnMatrix.set(2, 0, deltaRadians);
+        return returnMatrix;
+    }
+
+    /**
+     * This returns how far the robot has turned in radians, in a number not clamped between 0 and
+     * 2 * pi radians. This is used for some tuning things and nothing actually within the following.
+     *
+     * @return returns how far the robot has turned in total, in radians.
+     */
+    public double getTotalHeading() {
+        return totalHeading;
+    }
+
+    /**
+     * This returns the multiplier applied to forward movement measurement to convert from encoder
+     * ticks to inches. This is found empirically through a tuner.
+     *
+     * @return returns the forward ticks to inches multiplier
+     */
+    public double getForwardMultiplier() {
+        return FORWARD_TICKS_TO_INCHES;
+    }
+
+    /**
+     * This returns the multiplier applied to lateral/strafe movement measurement to convert from
+     * encoder ticks to inches. This is found empirically through a tuner.
+     *
+     * @return returns the lateral/strafe ticks to inches multiplier
+     */
+    public double getLateralMultiplier() {
+        return STRAFE_TICKS_TO_INCHES;
+    }
+
+    /**
+     * This returns the multiplier applied to turning movement measurement to convert from encoder
+     * ticks to radians. This is found empirically through a tuner.
+     *
+     * @return returns the turning ticks to radians multiplier
+     */
+    public double getTurningMultiplier() {
+        return 1;
+    }
+
+    /**
+     * This resets the IMU.
+     */
+
+    @Override
+    public void resetIMU() throws InterruptedException {
+        pinpoint.recalibrateIMU();
+
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+
+    /**
+     * This resets the pinpoint.
+     */
+    private void resetPinpoint() throws InterruptedException{
+        pinpoint.resetPosAndIMU();
+
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/ForwardTuner.java

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning;
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 
@@ -27,13 +26,13 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing;
  */
 @Config
 @Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning")
-public class FowardTuner extends OpMode {
+public class ForwardTuner extends OpMode {
     private PoseUpdater poseUpdater;
     private DashboardPoseTracker dashboardPoseTracker;
 
     private Telemetry telemetryA;
 
-    public static double DISTANCE = 30;
+    public static double DISTANCE = 48;
 
     /**
      * This initializes the PoseUpdater as well as the FTC Dashboard telemetry.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java

@@ -33,7 +33,7 @@ public class LateralTuner extends OpMode {
 
     private Telemetry telemetryA;
 
-    public static double DISTANCE = 30;
+    public static double DISTANCE = 48;
 
     /**
      * This initializes the PoseUpdater as well as the FTC Dashboard telemetry.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java

@@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -57,9 +60,10 @@ public class LocalizationTest extends OpMode {
         leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
         rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
-
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
 
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/SensorGoBildaPinpointExample.java

@@ -0,0 +1,200 @@
+/*   MIT License
+ *   Copyright (c) [2024] [Base 10 Assets, LLC]
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning;
+
+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.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
+
+import java.util.Locale;
+
+/*
+This opmode shows how to use the goBILDA® Pinpoint Odometry Computer.
+The goBILDA Odometry Computer is a device designed to solve the Pose Exponential calculation
+commonly associated with Dead Wheel Odometry systems. It reads two encoders, and an integrated
+system of senors to determine the robot's current heading, X position, and Y position.
+
+it uses an ESP32-S3 as a main cpu, with an STM LSM6DSV16X IMU.
+It is validated with goBILDA "Dead Wheel" Odometry pods, but should be compatible with any
+quadrature rotary encoder. The ESP32 PCNT peripheral is speced to decode quadrature encoder signals
+at a maximum of 40mhz per channel. Though the maximum in-application tested number is 130khz.
+
+The device expects two perpendicularly mounted Dead Wheel pods. The encoder pulses are translated
+into mm and their readings are transformed by an "offset", this offset describes how far away
+the pods are from the "tracking point", usually the center of rotation of the robot.
+
+Dead Wheel pods should both increase in count when moved forwards and to the left.
+The gyro will report an increase in heading when rotated counterclockwise.
+
+The Pose Exponential algorithm used is described on pg 181 of this book:
+https://github.com/calcmogul/controls-engineering-in-frc
+
+For support, contact tech@gobilda.com
+
+-Ethan Doak
+ */
+
+//TODO: If tuning comment out the @Disabled
+@TeleOp(name="goBILDA® PinPoint Odometry Example", group="Linear OpMode")
+@Disabled
+
+public class SensorGoBildaPinpointExample extends LinearOpMode {
+
+    GoBildaPinpointDriver odo; // Declare OpMode member for the Odometry Computer
+
+    double oldTime = 0;
+
+
+    @Override
+    public void runOpMode() {
+
+        // Initialize the hardware variables. Note that the strings used here must correspond
+        // to the names assigned during the robot configuration step on the DS or RC devices.
+
+        odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo");
+
+        /*
+        Set the odometry pod positions relative to the point that the odometry computer tracks around.
+        The X pod offset refers to how far sideways from the tracking point the
+        X (forward) odometry pod is. Left of the center is a positive number,
+        right of center is a negative number. the Y pod offset refers to how far forwards from
+        the tracking point the Y (strafe) odometry pod is. forward of center is a positive number,
+        backwards is a negative number.
+         */
+        odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1
+
+        /*
+        Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
+        the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD.
+        If you're using another kind of odometry pod, uncomment setEncoderResolution and input the
+        number of ticks per mm of your odometry pod.
+         */
+        odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
+        //odo.setEncoderResolution(13.26291192);
+
+
+        /*
+        Set the direction that each of the two odometry pods count. The X (forward) pod should
+        increase when you move the robot forward. And the Y (strafe) pod should increase when
+        you move the robot to the left.
+         */
+        odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);
+
+
+        /*
+        Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
+        The IMU will automatically calibrate when first powered on, but recalibrating before running
+        the robot is a good idea to ensure that the calibration is "good".
+        resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
+        This is recommended before you run your autonomous, as a bad initial calibration can cause
+        an incorrect starting value for x, y, and heading.
+         */
+        //odo.recalibrateIMU();
+        odo.resetPosAndIMU();
+
+        telemetry.addData("Status", "Initialized");
+        telemetry.addData("X offset", odo.getXOffset());
+        telemetry.addData("Y offset", odo.getYOffset());
+        telemetry.addData("Device Version Number:", odo.getDeviceVersion());
+        telemetry.addData("Device Scalar", odo.getYawScalar());
+        telemetry.update();
+
+        // Wait for the game to start (driver presses START)
+        waitForStart();
+        resetRuntime();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            /*
+            Request an update from the Pinpoint odometry computer. This checks almost all outputs
+            from the device in a single I2C read.
+             */
+            odo.update();
+
+            /*
+            Optionally, you can update only the heading of the device. This takes less time to read, but will not
+            pull any other data. Only the heading (which you can pull with getHeading() or in getPosition().
+             */
+            //odo.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING);
+
+
+            if (gamepad1.a){
+                odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU
+            }
+
+            if (gamepad1.b){
+                odo.recalibrateIMU(); //recalibrates the IMU without resetting position
+            }
+
+            /*
+            This code prints the loop frequency of the REV Control Hub. This frequency is effected
+            by I²C reads/writes. So it's good to keep an eye on. This code calculates the amount
+            of time each cycle takes and finds the frequency (number of updates per second) from
+            that cycle time.
+             */
+            double newTime = getRuntime();
+            double loopTime = newTime-oldTime;
+            double frequency = 1/loopTime;
+            oldTime = newTime;
+
+
+            /*
+            gets the current Position (x & y in mm, and heading in degrees) of the robot, and prints it.
+             */
+            Pose2D pos = odo.getPosition();
+            String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES));
+            telemetry.addData("Position", data);
+
+            /*
+            gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and prints it.
+             */
+            Pose2D vel = odo.getVelocity();
+            String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.getX(DistanceUnit.MM), vel.getY(DistanceUnit.MM), vel.getHeading(AngleUnit.DEGREES));
+            telemetry.addData("Velocity", velocity);
+
+
+            /*
+            Gets the Pinpoint device status. Pinpoint can reflect a few states. But we'll primarily see
+            READY: the device is working as normal
+            CALIBRATING: the device is calibrating and outputs are put on hold
+            NOT_READY: the device is resetting from scratch. This should only happen after a power-cycle
+            FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in
+            FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in
+            FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in
+            */
+            telemetry.addData("Status", odo.getDeviceStatus());
+
+            telemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint
+
+            telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate
+            telemetry.update();
+
+        }
+    }}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java

@@ -27,16 +27,16 @@ public class MathFunctions {
         double denom = 1;
 
         // this multiplies up the numerator of the nCr function
-        for (int i = n; i > n-r; i--) {
+        for (int i = n; i > n - r; i--) {
             num *= i;
         }
 
         // this multiplies up the denominator of the nCr function
-        for (int i = 1; i <=r; i++) {
+        for (int i = 1; i <= r; i++) {
             denom *= i;
         }
 
-        return num/denom;
+        return num / denom;
     }
 
     /**
@@ -67,7 +67,7 @@ public class MathFunctions {
 
     /**
      * This normalizes an angle to be between 0 and 2 pi radians, inclusive.
-     *
+     * <p>
      * IMPORTANT NOTE: This method operates in radians.
      *
      * @param angleRadians the angle to be normalized.
@@ -75,8 +75,8 @@ public class MathFunctions {
      */
     public static double normalizeAngle(double angleRadians) {
         double angle = angleRadians;
-        while (angle<0) angle += 2*Math.PI;
-        while (angle>2*Math.PI) angle -= 2*Math.PI;
+        while (angle < 0) angle += 2 * Math.PI;
+        while (angle > 2 * Math.PI) angle -= 2 * Math.PI;
         return angle;
     }
 
@@ -88,7 +88,7 @@ public class MathFunctions {
      * @return returns the smallest angle.
      */
     public static double getSmallestAngleDifference(double one, double two) {
-        return Math.min(MathFunctions.normalizeAngle(one-two), MathFunctions.normalizeAngle(two-one));
+        return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one));
     }
 
     /**
@@ -98,7 +98,7 @@ public class MathFunctions {
      * @return returns the turn direction.
      */
     public static double getTurnDirection(double startHeading, double endHeading) {
-        if (MathFunctions.normalizeAngle(endHeading-startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading-startHeading) <= Math.PI) {
+        if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) {
             return 1; // counter clock wise
         }
         return -1; // clock wise
@@ -112,7 +112,7 @@ public class MathFunctions {
      * @return returns the distance between the two.
      */
     public static double distance(Pose pose, Point point) {
-        return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2));
+        return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2));
     }
 
     /**
@@ -123,7 +123,7 @@ public class MathFunctions {
      * @return returns the distance between the two.
      */
     public static double distance(Pose one, Pose two) {
-        return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2));
+        return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2));
     }
 
     /**
@@ -172,6 +172,22 @@ public class MathFunctions {
         return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading());
     }
 
+    /**
+     * This rotates the given pose by the given theta,
+     *
+     * @param pose the Pose to rotate.
+     * @param theta the angle to rotate by.
+     * @param rotateHeading whether to adjust the Pose heading too.
+     * @return the rotated Pose.
+     */
+    public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) {
+        double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta);
+        double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta);
+        double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading();
+
+        return new Pose(x, y, heading);
+    }
+
     /**
      * This multiplies a Point by a scalar and returns the result as a Point
      *
@@ -180,7 +196,7 @@ public class MathFunctions {
      * @return returns the scaled Point.
      */
     public static Point scalarMultiplyPoint(Point point, double scalar) {
-        return new Point(point.getX()*scalar, point.getY()*scalar, Point.CARTESIAN);
+        return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN);
     }
 
     /**
@@ -213,7 +229,7 @@ public class MathFunctions {
      * @return returns the scaled Vector.
      */
     public static Vector scalarMultiplyVector(Vector vector, double scalar) {
-        return new Vector(vector.getMagnitude()*scalar, vector.getTheta());
+        return new Vector(vector.getMagnitude() * scalar, vector.getTheta());
     }
 
     /**
@@ -227,7 +243,7 @@ public class MathFunctions {
         if (vector.getMagnitude() == 0) {
             return new Vector(0.0, vector.getTheta());
         } else {
-            return new Vector(vector.getMagnitude()/Math.abs(vector.getMagnitude()), vector.getTheta());
+            return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta());
         }
     }
 
@@ -240,7 +256,7 @@ public class MathFunctions {
      */
     public static Vector addVectors(Vector one, Vector two) {
         Vector returnVector = new Vector();
-        returnVector.setOrthogonalComponents(one.getXComponent()+two.getXComponent(), one.getYComponent()+two.getYComponent());
+        returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent());
         return returnVector;
     }
 
@@ -254,7 +270,7 @@ public class MathFunctions {
      */
     public static Vector subtractVectors(Vector one, Vector two) {
         Vector returnVector = new Vector();
-        returnVector.setOrthogonalComponents(one.getXComponent()-two.getXComponent(), one.getYComponent()-two.getYComponent());
+        returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent());
         return returnVector;
     }
 
@@ -266,7 +282,7 @@ public class MathFunctions {
      * @return returns the dot product of the two Vectors.
      */
     public static double dotProduct(Vector one, Vector two) {
-        return one.getXComponent()*two.getXComponent() + one.getYComponent()*two.getYComponent();
+        return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent();
     }
 
     /**
@@ -278,7 +294,7 @@ public class MathFunctions {
      * @return returns the cross product of the two Vectors.
      */
     public static double crossProduct(Vector one, Vector two) {
-        return one.getXComponent()*two.getYComponent() - one.getYComponent()*two.getXComponent();
+        return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent();
     }
 
     /**
@@ -286,7 +302,7 @@ public class MathFunctions {
      * specified accuracy amount.
      *
      * @param one first number specified.
-     * @param two second number specified.
+     * @param two Second number specified.
      * @param accuracy the level of accuracy specified.
      * @return returns if the two numbers are within the specified accuracy of each other.
      */

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java

@@ -51,16 +51,47 @@ public class PathBuilder {
         return this;
     }
 
+    /**
+     * This adds a default Path defined by a specified BezierCurve to the PathBuilder.
+     *
+     * @param controlPoints This is the specified control points that define the BezierCurve.
+     * @return This returns itself with the updated data.
+     */
+    public PathBuilder addBezierCurve(Point... controlPoints) {
+        return addPath(new BezierCurve(controlPoints));
+    }
+
+    /**
+     * This adds a default Path defined by a specified BezierCurve to the PathBuilder.
+     *
+     * @param controlPoints This is the specified control points that define the BezierCurve.
+     * @return This returns itself with the updated data.
+     */
+    public PathBuilder addBezierCurve(ArrayList<Point> controlPoints) {
+        return addPath(new BezierCurve(controlPoints));
+    }
+
+    /**
+     * This adds a default Path defined by a specified BezierLine to the PathBuilder.
+     *
+     * @param startPoint start point of the line.
+     * @param endPoint end point of the line.
+     * @return This returns itself with the updated data.
+     */
+    public PathBuilder addBezierLine(Point startPoint, Point endPoint) {
+        return addPath(new BezierLine(startPoint, endPoint));
+    }
+
     /**
      * This sets a linear heading interpolation on the last Path added to the PathBuilder.
      *
      * @param startHeading The start of the linear heading interpolation.
      * @param endHeading The end of the linear heading interpolation.
-     *                   This will be reached at the end of the Path if no end time is specified.
+     *         This will be reached at the end of the Path if no end time is specified.
      * @return This returns itself with the updated data.
      */
     public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) {
-        this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading);
+        this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading);
         return this;
     }
 
@@ -69,13 +100,13 @@ public class PathBuilder {
      *
      * @param startHeading The start of the linear heading interpolation.
      * @param endHeading The end of the linear heading interpolation.
-     *                   This will be reached at the end of the Path if no end time is specified.
+     *         This will be reached at the end of the Path if no end time is specified.
      * @param endTime The end time on the Path that the linear heading interpolation will end.
-     *                This value goes from [0, 1] since Bezier curves are parametric functions.
+     *         This value goes from [0, 1] since Bezier curves are parametric functions.
      * @return This returns itself with the updated data.
      */
     public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) {
-        this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading, endTime);
+        this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime);
         return this;
     }
 
@@ -86,7 +117,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setConstantHeadingInterpolation(double setHeading) {
-        this.paths.get(paths.size()-1).setConstantHeadingInterpolation(setHeading);
+        this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading);
         return this;
     }
 
@@ -94,20 +125,21 @@ public class PathBuilder {
      * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder.
      *
      * @param set This sets the heading to reversed tangent following if this parameter is true.
-     *            Conversely, this sets a tangent following if this parameter is false.
+     *         Conversely, this sets a tangent following if this parameter is false.
      * @return This returns itself with the updated data.
      */
     public PathBuilder setReversed(boolean set) {
-        this.paths.get(paths.size()-1).setReversed(set);
+        this.paths.get(paths.size() - 1).setReversed(set);
         return this;
     }
+
     /**
      * This sets the heading interpolation to tangential on the last Path added to the PathBuilder.
      * There really shouldn't be a reason to use this since the default heading interpolation is
      * tangential but it's here.
      */
     public PathBuilder setTangentHeadingInterpolation() {
-        this.paths.get(paths.size()-1).setTangentHeadingInterpolation();
+        this.paths.get(paths.size() - 1).setTangentHeadingInterpolation();
         return this;
     }
 
@@ -118,7 +150,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setZeroPowerAccelerationMultiplier(double set) {
-        this.paths.get(paths.size()-1).setZeroPowerAccelerationMultiplier(set);
+        this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set);
         return this;
     }
 
@@ -129,7 +161,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndVelocityConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndVelocityConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set);
         return this;
     }
 
@@ -140,7 +172,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndTranslationalConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndTranslationalConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set);
         return this;
     }
 
@@ -151,7 +183,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndHeadingConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndHeadingConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set);
         return this;
     }
 
@@ -162,7 +194,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndTValueConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndTValueConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set);
         return this;
     }
 
@@ -173,7 +205,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndTimeoutConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndTimeoutConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set);
         return this;
     }
 
@@ -182,12 +214,12 @@ public class PathBuilder {
      * This callback is set to run at a specified number of milliseconds after the start of the path.
      *
      * @param time This sets the number of milliseconds of wait between the start of the Path and
-     *             the calling of the callback.
+     *         the calling of the callback.
      * @param runnable This sets the code for the callback to run. Use lambda statements for this.
      * @return This returns itself with the updated data.
      */
     public PathBuilder addTemporalCallback(double time, Runnable runnable) {
-        this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size()-1));
+        this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1));
         return this;
     }
 
@@ -200,7 +232,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder addParametricCallback(double t, Runnable runnable) {
-        this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size()-1));
+        this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1));
         return this;
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration;
 
+import androidx.annotation.NonNull;
+
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
 
 /**
@@ -31,13 +33,13 @@ public class Point {
      * This creates a new Point with coordinate inputs and a specified coordinate system.
      *
      * @param rOrX Depending on the coordinate system specified, this is either the r or x value.
-     *             In polar coordinates, the r value is the distance from the origin.
-     *             In Cartesian coordinates, the x value is the distance left/right from the origin.
+     *         In polar coordinates, the r value is the distance from the origin.
+     *         In Cartesian coordinates, the x value is the distance left/right from the origin.
      * @param thetaOrY Depending on the coordinate system specified, this is either the theta or
-     *                 y value.
-     *                 In polar coordinates, the theta value is the angle from the positive x-axis.
-     *                 Increasing theta moves in the counter-clockwise direction.
-     *                 In Cartesian coordinates, the y value is the distance up/down from the origin.
+     *         y value.
+     *         In polar coordinates, the theta value is the angle from the positive x-axis.
+     *         Increasing theta moves in the counter-clockwise direction.
+     *         In Cartesian coordinates, the y value is the distance up/down from the origin.
      * @param identifier this specifies what coordinate system the coordinate inputs are in.
      */
     public Point(double rOrX, double thetaOrY, int identifier) {
@@ -53,17 +55,27 @@ public class Point {
         setCoordinates(pose.getX(), pose.getY(), CARTESIAN);
     }
 
+    /**
+     * This creates a new Point from a X and Y value.
+     *
+     * @param setX the X value.
+     * @param setY the Y value.
+     */
+    public Point(double setX, double setY) {
+        setCoordinates(setX, setY, CARTESIAN);
+    }
+
     /**
      * This sets the coordinates of the Point using the specified coordinate system.
      *
      * @param rOrX Depending on the coordinate system specified, this is either the r or x value.
-     *             In polar coordinates, the r value is the distance from the origin.
-     *             In Cartesian coordinates, the x value is the distance left/right from the origin.
+     *         In polar coordinates, the r value is the distance from the origin.
+     *         In Cartesian coordinates, the x value is the distance left/right from the origin.
      * @param thetaOrY Depending on the coordinate system specified, this is either the theta or
-     *                 y value.
-     *                 In polar coordinates, the theta value is the angle from the positive x-axis.
-     *                 Increasing theta moves in the counter-clockwise direction.
-     *                 In Cartesian coordinates, the y value is the distance up/down from the origin.
+     *         y value.
+     *         In polar coordinates, the theta value is the angle from the positive x-axis.
+     *         Increasing theta moves in the counter-clockwise direction.
+     *         In Cartesian coordinates, the y value is the distance up/down from the origin.
      * @param identifier this specifies what coordinate system to use when setting values.
      */
     public void setCoordinates(double rOrX, double thetaOrY, int identifier) {
@@ -78,9 +90,9 @@ public class Point {
                 theta = setOtherCoordinates[1];
                 break;
             default:
-                if (rOrX<0) {
+                if (rOrX < 0) {
                     r = -rOrX;
-                    theta = MathFunctions.normalizeAngle(thetaOrY+Math.PI);
+                    theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI);
                 } else {
                     r = rOrX;
                     theta = MathFunctions.normalizeAngle(thetaOrY);
@@ -99,7 +111,7 @@ public class Point {
      * @return returns the distance between the two Points.
      */
     public double distanceFrom(Point otherPoint) {
-        return Math.sqrt(Math.pow(otherPoint.getX()-x, 2) + Math.pow(otherPoint.getY()-y, 2));
+        return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2));
     }
 
     /**
@@ -110,7 +122,7 @@ public class Point {
      * @return this returns the x and y values, in that order, in an Array of doubles.
      */
     public static double[] polarToCartesian(double r, double theta) {
-        return new double[] {r * Math.cos(theta), r * Math.sin(theta)};
+        return new double[]{r * Math.cos(theta), r * Math.sin(theta)};
     }
 
     /**
@@ -123,17 +135,17 @@ public class Point {
     public static double[] cartesianToPolar(double x, double y) {
         if (x == 0) {
             if (y > 0) {
-                return new double[] {Math.abs(y), Math.PI/2};
+                return new double[]{Math.abs(y), Math.PI / 2};
             } else {
-                return new double[] {Math.abs(y), (3 * Math.PI) / 2};
+                return new double[]{Math.abs(y), (3 * Math.PI) / 2};
             }
         }
-        double r = Math.sqrt(x*x+y*y);
-        if (x < 0) return new double[] {r, Math.PI+Math.atan(y/x)};
+        double r = Math.sqrt(x * x + y * y);
+        if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)};
         if (y > 0) {
             return new double[]{r, Math.atan(y / x)};
         } else {
-            return new double[]{r, (2*Math.PI) + Math.atan(y / x)};
+            return new double[]{r, (2 * Math.PI) + Math.atan(y / x)};
         }
     }
 
@@ -182,4 +194,10 @@ public class Point {
     public Point copy() {
         return new Point(getX(), getY(), CARTESIAN);
     }
+
+    @NonNull
+    @Override
+    public String toString() {
+        return "(" + getX() + ", " + getY() + ")";
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning;
 import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients;
 import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients;
@@ -30,6 +31,11 @@ public class FollowerConstants {
     public static String rightFrontMotorName = FRONT_RIGHT_MOTOR;
     public static String rightRearMotorName = BACK_RIGHT_MOTOR;
 
+    public static DcMotorSimple.Direction leftFrontMotorDirection = FRONT_LEFT_MOTOR_DIRECTION;
+    public static DcMotorSimple.Direction rightFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION;
+    public static DcMotorSimple.Direction leftRearMotorDirection = BACK_LEFT_MOTOR_DIRECTION;
+    public static DcMotorSimple.Direction rightRearMotorDirection = BACK_RIGHT_MOTOR_DIRECTION;
+
     // This section is for setting the actual drive vector for the front left wheel, if the robot
     // is facing a heading of 0 radians with the wheel centered at (0,0)
     private static final double xMovement = ROBOT_SPEED_FORWARD;
@@ -40,9 +46,9 @@ public class FollowerConstants {
 
     // Translational PIDF coefficients (don't use integral)
     public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
-            0.1,
+            .25,
             0,
-            0.01,
+            0.02,
             0);
 
     // Translational Integral
@@ -60,7 +66,7 @@ public class FollowerConstants {
     public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
             2,
             0,
-            0.025,
+            0.0375,
             0);
 
     // Feed forward constant added on to the heading PIDF
@@ -69,9 +75,9 @@ public class FollowerConstants {
 
     // Drive PIDF coefficients
     public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
-            0.006,
+            0.00235,
             0,
-            0.00001,
+            0.00003,
             0.8,
             0);
 
@@ -184,7 +190,7 @@ public class FollowerConstants {
 
 
     // the limit at which the heading PIDF switches between the main and secondary heading PIDFs
-    public static double headingPIDFSwitch = Math.PI/20;
+    public static double headingPIDFSwitch = Math.PI / 20;
 
     // Secondary heading error PIDF coefficients
     public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients(

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java

@@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -40,7 +43,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning")
 public class ForwardVelocityTuner extends OpMode {
     private ArrayList<Double> velocities = new ArrayList<>();
 
@@ -52,7 +55,7 @@ public class ForwardVelocityTuner extends OpMode {
 
     private PoseUpdater poseUpdater;
 
-    public static double DISTANCE = 40;
+    public static double DISTANCE = 48;
     public static double RECORD_NUMBER = 10;
 
     private Telemetry telemetryA;
@@ -71,10 +74,10 @@ public class ForwardVelocityTuner extends OpMode {
         leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
         rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
-
-        // TODO: Make sure that this is the direction your motors need to be reversed in.
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
 
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
 
@@ -93,13 +96,11 @@ public class ForwardVelocityTuner extends OpMode {
         }
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-//        telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
-//        telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
-//        telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
-//        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
-//
-//
-//        telemetryA.update();
+        telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
+        telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
+        telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
+        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
+        telemetryA.update();
 
     }
 
@@ -124,6 +125,10 @@ public class ForwardVelocityTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 
@@ -140,14 +145,14 @@ public class ForwardVelocityTuner extends OpMode {
                 velocities.add(currentVelocity);
                 velocities.remove(0);
             }
-            telemetryA.addData("x", poseUpdater.getPose().getX());
-            telemetryA.addData("y", poseUpdater.getPose().getY());
-            telemetryA.addData("heading", poseUpdater.getPose().getHeading());
-            telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
-            telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
-            telemetryA.update();
-
         } else {
+            leftFront.setPower(0);
+            leftRear.setPower(0);
+            rightFront.setPower(0);
+            rightRear.setPower(0);
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            }
             double average = 0;
             for (Double velocity : velocities) {
                 average += velocity;
@@ -155,12 +160,7 @@ public class ForwardVelocityTuner extends OpMode {
             average /= (double) velocities.size();
 
             telemetryA.addData("forward velocity:", average);
-//            telemetryA.addData("x", poseUpdater.getPose().getX());
-//            telemetryA.addData("y", poseUpdater.getPose().getY());
-//            telemetryA.addData("heading", poseUpdater.getPose().getHeading());
-//            telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
-//            telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
-//            telemetryA.update();
+            telemetryA.update();
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java

@@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -40,7 +43,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
 public class ForwardZeroPowerAccelerationTuner extends OpMode {
     private ArrayList<Double> accelerations = new ArrayList<>();
 
@@ -74,10 +77,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode {
         leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
         rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
-
-        // TODO: Make sure that this is the direction your motors need to be reversed in.
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
 
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
 
@@ -120,6 +123,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 
@@ -149,7 +156,7 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode {
             for (Double acceleration : accelerations) {
                 average += acceleration;
             }
-            average /= (double)accelerations.size();
+            average /= (double) accelerations.size();
 
             telemetryA.addData("forward zero power acceleration (deceleration):", average);
             telemetryA.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java

@@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -40,7 +43,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
 public class LateralZeroPowerAccelerationTuner extends OpMode {
     private ArrayList<Double> accelerations = new ArrayList<>();
 
@@ -52,7 +55,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
 
     private PoseUpdater poseUpdater;
 
-    public static double VELOCITY = 50;
+    public static double VELOCITY = 30;
 
     private double previousVelocity;
 
@@ -74,10 +77,10 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
         leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
         rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
-
-        // TODO: Make sure that this is the direction your motors need to be reversed in.
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
 
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
 
@@ -120,11 +123,15 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 
         poseUpdater.update();
-        Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI/2);
+        Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2);
         if (!end) {
             if (!stopping) {
                 if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) {
@@ -149,7 +156,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
             for (Double acceleration : accelerations) {
                 average += acceleration;
             }
-            average /= (double)accelerations.size();
+            average /= (double) accelerations.size();
 
             telemetryA.addData("lateral zero power acceleration (deceleration):", average);
             telemetryA.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java

@@ -4,6 +4,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorDirection;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorDirection;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -12,7 +16,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -40,7 +43,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning")
 public class StrafeVelocityTuner extends OpMode {
     private ArrayList<Double> velocities = new ArrayList<>();
 
@@ -52,7 +55,7 @@ public class StrafeVelocityTuner extends OpMode {
 
     private PoseUpdater poseUpdater;
 
-    public static double DISTANCE = 40;
+    public static double DISTANCE = 48;
     public static double RECORD_NUMBER = 10;
 
     private Telemetry telemetryA;
@@ -71,10 +74,10 @@ public class StrafeVelocityTuner extends OpMode {
         leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
         rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
-
-        // TODO: Make sure that this is the direction your motors need to be reversed in.
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftFront.setDirection(leftFrontMotorDirection);
+        leftRear.setDirection(leftRearMotorDirection);
+        rightFront.setDirection(rightFrontMotorDirection);
+        rightRear.setDirection(rightRearMotorDirection);
 
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
 
@@ -120,6 +123,10 @@ public class StrafeVelocityTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 
@@ -132,11 +139,18 @@ public class StrafeVelocityTuner extends OpMode {
                     motor.setPower(0);
                 }
             } else {
-                double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI/2)));
+                double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2)));
                 velocities.add(currentVelocity);
                 velocities.remove(0);
             }
         } else {
+            leftFront.setPower(0);
+            leftRear.setPower(0);
+            rightFront.setPower(0);
+            rightRear.setPower(0);
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            }
             double average = 0;
             for (Double velocity : velocities) {
                 average += velocity;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java

@@ -20,11 +20,14 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
  * @version 1.0, 4/22/2024
  */
 public class Drawing {
+    public static final double ROBOT_RADIUS = 9;
+
     private static TelemetryPacket packet;
 
     /**
      * This draws everything that will be used in the Follower's telemetryDebug() method. This takes
      * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed.
+     *
      * @param follower
      */
     public static void drawDebug(Follower follower) {
@@ -35,6 +38,7 @@ public class Drawing {
         }
         drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50");
         drawRobot(follower.getPose(), "#4CAF50");
+
         sendPacket();
     }
 
@@ -114,12 +118,10 @@ public class Drawing {
      * @param t the Point to draw at
      */
     public static void drawRobotOnCanvas(Canvas c, Point t) {
-        final double ROBOT_RADIUS = 9;
-
         c.setStrokeWidth(1);
         c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS);
 
-        Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta());
+        Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta());
         Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta()));
         Vector p2 = MathFunctions.addVectors(p1, halfv);
         c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent());
@@ -133,8 +135,6 @@ public class Drawing {
      * @param t the Pose to draw at
      */
     public static void drawRobotOnCanvas(Canvas c, Pose t) {
-        final double ROBOT_RADIUS = 9;
-
         c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS);
         Vector v = t.getHeadingVector();
         v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/BlueBasketAutoStates.java

@@ -1,11 +1,9 @@
-package org.firstinspires.ftc.teamcode.cometbots.projects;
+package org.firstinspires.ftc.teamcode.states;
 
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
@@ -15,32 +13,16 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 
-/**
- * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
- * a circle, but some Bezier curves that have control points set essentially in a square. However,
- * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
- * heading is to be expected.
- *
- * @author Anyi Lin - 10158 Scott's Bots
- * @author Aaron Yang - 10158 Scott's Bots
- * @author Harrison Womack - 10158 Scott's Bots
- * @version 1.0, 3/12/2024
- */
-@Config
-@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
-public class BlueBasketAuto extends OpMode {
+@Autonomous(name = "Blue Basket Auto States", group = "Competition")
+public class BlueBasketAutoStates extends OpMode {
     private Telemetry telemetryA;
 
     private Follower follower;
 
     private PathChain path;
 
-    private final Pose startPose = new Pose(11.25, 95.75, 0);
+    private final Pose startPose = new Pose(11.25, 95.75);
 
-    /**
-     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
-     * initializes the FTC Dashboard telemetry.
-     */
     @Override
     public void init() {
         follower = new Follower(hardwareMap);
@@ -133,29 +115,22 @@ public class BlueBasketAuto extends OpMode {
                         )
                 )
                 .addPath(
-                        // Line 9
+                        // Line 11
                         new BezierLine(
                                 new Point(18.643, 135.000, Point.CARTESIAN),
                                 new Point(83.250, 95.464, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
-        follower.followPath(path, true);
+        follower.followPath(path);
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
         telemetryA.update();
     }
 
-    /**
-     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
-     * the Telemetry, as well as the FTC Dashboard.
-     */
     @Override
     public void loop() {
         follower.update();
-        if (follower.atParametricEnd()) {
-            follower.followPath(path, true);
-        }
         follower.telemetryDebug(telemetryA);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine3.java

@@ -1,73 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystem;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-
-/*
-    AutoLine# - This file does something of a path......
-
- */
-public class AutoLine3 implements Action {
-
-    private Follower actionRobot;
-    private PathChain pathChain;
-
-    private Pose startPose = new Pose(56,24);
-
-    public AutoLine3(Follower robot) {
-        this.actionRobot = robot;
-        this.actionRobot.setStartingPose(startPose);
-
-        PathBuilder builder = new PathBuilder();
-        builder
-         /*       .addPath(
-                        // Line 1
-                        new BezierLine(
-                                new Point(8.000, 65.000, Point.CARTESIAN),
-                                new Point(30.000, 72.000, Point.CARTESIAN)
-//           This is the origional                     new Point(36.000, 72.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 2
-                        new BezierCurve(
-                                new Point(36.000, 72.000, Point.CARTESIAN),
-                                new Point(24.000, 24.000, Point.CARTESIAN),
-                                new Point(72.000, 36.000, Point.CARTESIAN),
-                                new Point(56.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))          */
-                .addPath(
-                   // Line 3
-                   new BezierLine(
-                            new Point(56.000, 24.000, Point.CARTESIAN),
-                             new Point(18.000, 24.000, Point.CARTESIAN)
-                        )
-                )
-                .setTangentHeadingInterpolation();
-
-        pathChain = builder.build();
-
-        this.actionRobot.followPath(this.pathChain);
-    }
-
-    @Override
-    public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-        this.actionRobot.update();
-        return this.actionRobot.isBusy();
-    }
-}
-

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java

@@ -0,0 +1,92 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armReverseBucket;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armSpecimen;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+
+public class ArmSubsystem {
+
+    public enum ArmState {
+        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET, SPECIMEN
+    }
+
+    private ServoImplEx arm;
+    private ArmState state;
+
+    public ArmSubsystem(HardwareMap hardwareMap) {
+        this.arm = hardwareMap.get(ServoImplEx.class, ARM_SERVO);
+    }
+    public void grabBlueberrySkyhook(){
+        setState(ArmState.GRAB_BLUEBERRY_SKYHOOK);
+        arm.setPosition(armGrabBlueberrySkyhook);
+    }
+    public void hangBlueberrySkyhook(){
+        setState(ArmState.HANG_BLUEBERRY_SKYHOOK);
+        arm.setPosition(armHangBlueberrySkyhook);
+    }
+    public void toReverseBucket(){
+        arm.setPosition(armReverseBucket);
+        setState(ArmState.REVERE_BUCKET);
+    }
+    public void toParkPosition() {
+        arm.setPosition(armPark);
+        setState(ArmState.PARK);
+    }
+
+    public void toFloorPosition() {
+        arm.setPosition(armFloor);
+        setState(ArmState.FLOOR);
+    }
+    public void toFloorPositionTeleOp() {
+        arm.setPosition(.68);
+        setState(ArmState.FLOOR);
+    }
+
+    public void toBucketPosition() {
+        arm.setPosition(armBucket);
+        setState(ArmState.BUCKET);
+    }
+    public void toSpecimenPrep() {
+        arm.setPosition(armSpecimen);
+        setState(ArmState.SPECIMEN);
+    }
+    public void toInitPosition(){
+        arm.setPosition(armInit);
+        setState(ArmState.INIT);
+    }
+
+    public void setState(ArmState armState) {
+        this.state = armState;
+    }
+
+    public ArmState getState() {
+        return this.state;
+    }
+
+    public void initTeleOp() {
+        arm.resetDeviceConfigurationForOpMode();
+        toParkPosition();
+    }
+    public void initAuto(){
+        arm.resetDeviceConfigurationForOpMode();
+        toInitPosition();
+    }
+
+    public double getPosition() {
+        return this.arm.getPosition();
+    }
+
+    public void setPosition(double position) {
+        this.arm.setPosition(position);
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ClawSubsystem.java

@@ -0,0 +1,98 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
+import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.autoClawOpen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.grabBlueberry;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class ClawSubsystem {
+
+    public enum ClawState {
+        CLOSED, OPEN, GRAB_BLUEBERRY, AUTO_OPEN
+    }
+
+    public enum ThumbState {
+        DOWN, UP, AWAY
+    }
+
+    private Servo claw;
+    private Servo thumb;
+    private ClawState state;
+    private ThumbState tState;
+
+    public ClawSubsystem(HardwareMap hardwareMap) {
+        claw = hardwareMap.get(Servo.class, CLAW_NAME);
+        thumb = hardwareMap.get(Servo.class, THUMB_SERVO);
+    }
+    public void grabBlueberry(){
+        claw.setPosition(grabBlueberry);
+        state = ClawState.GRAB_BLUEBERRY;
+    }
+
+    public void closeClaw() {
+        claw.setPosition(clawClose);
+
+        state = ClawState.CLOSED;
+    }
+
+    public void openClaw() {
+        claw.setPosition(clawOpen);
+
+        state = ClawState.OPEN;
+    }
+    public void autoOpenClaw() {
+        claw.setPosition(autoClawOpen);
+
+        state = ClawState.AUTO_OPEN;
+    }
+
+    public ClawState getState() {
+        return state;
+    }
+
+    public ThumbState getTState() {
+        return tState;
+    }
+
+    public void switchState() {
+        if (state == ClawState.CLOSED) {
+            openClaw();
+        } else if (state == ClawState.OPEN) {
+            closeClaw();
+        }
+    }
+
+    public void switchTState() {
+        if (tState == ThumbState.UP) {
+            thumbDown();
+        } else if (tState == ThumbState.DOWN) {
+            thumbUp();
+        }
+    }
+
+    public void init() {
+        closeClaw();
+        thumbDown();
+    }
+
+    public void thumbUp() {
+        thumb.setPosition(0.5);
+        tState = ThumbState.UP;
+    }
+
+    public void thumbDown() {
+        thumb.setPosition(0.00);
+        tState = ThumbState.DOWN;
+    }
+
+    public void thumbOutOfTheWay() {
+        thumb.setPosition(0.95);
+        tState = ThumbState.AWAY;
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DualMotorSliderSubsystem.java

@@ -0,0 +1,203 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.backwardBucketDrop;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideBelowFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanHang;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanRelease;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class DualMotorSliderSubsystem {
+
+    /*
+    liftSlideLeft - Left Motor for Dual Linear Slide as a DcMotorEx object
+    Currently, the value of (liftSlideLeft) is null because we haven't assigned a value (object) to it yet.
+    It expects an object of type "DcMotorEx".
+     */
+    private DcMotorEx liftSlideLeft;
+
+    /*
+    liftSlideRight - Right Motor for Dual Linear Slide as a DcMotorEx object
+    Currently, the value of (liftSlideRight) is null because we haven't assigned a value (object) to it yet.
+    It expects an object of type "DcMotorEx".
+     */
+    private DcMotorEx liftSlideRight;
+
+    /*
+    targetPosition - Variable that holds target position of slides.
+    */
+    private int targetPosition = 0;
+
+    /*
+    getTargetPosition/setTargetPosition - Best practice to "hide" (private) targetPosition and,
+                                          instead, use a "setter" to set the target position value or
+                                          a "getter" to get the target position value.
+    */
+    public void setTargetPosition(int value) {
+        if(value >= liftToHighBucketPos)
+            targetPosition = liftToHighBucketPos;
+        else if(targetPosition <= liftToHighBucketPos)
+            targetPosition = value;
+    }
+    private int getTargetPosition() { return targetPosition; }
+
+    /*
+    PID - Proportional/Integral/Derivative Values
+
+    For a dual motor linear slide, we only tune the P - Proportion.
+    The Proportion variable (kp) answers the question "how fast do we want to get to our destination?"
+
+    It's the only value we set because the variable ki and kd deal with how to handle when we're off the path.
+    Since we're going straight, we don't need to worry about.
+    */
+    public final static double kp = 0.002, ki = 0, kd = 0;
+
+    /*
+    lastError/integralSum/timer - These 3 variables are placeholders in determining how much
+                                  power to send to both motors.
+    */
+    private double lastError = 0;
+    private double integralSum = 0;
+    private ElapsedTime timer = new ElapsedTime();
+
+    public DualMotorSliderSubsystem(HardwareMap hardwareMap) {
+        /*
+        liftSlideLeft/liftSlideRight - Now, we are assigning a value of DcMotorEx to each variable
+
+        We "assign" the object DcMotorEx to liftSlideLeft and liftSlideRight and "link them" to the
+        driver hub configuration name that matches the motor on the slide.
+         */
+        liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
+        liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
+    }
+    public int getCurrentPosition(){
+        return liftSlideLeft.getCurrentPosition();
+    }
+    public double getCurrentPower(){
+        return liftSlideLeft.getPower();
+    }
+    public void init() {
+        /*
+        Initialize the motors with the following settings (assuming slide is at the very bottom position):
+
+        - Reset the encoders to be zero
+        - When the motor stops moving (zero power), brake.  This means we can't move the motors, not even gravity.
+        - Sets the motor to run without the encoder.  This doesn't mean we won't use the encoder values (because we will).
+          It just means to not FULLY depend on them, we will just prefer to use motor power instead.
+         */
+        liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        //rotation is different because spools spin opposite directions now
+        liftSlideLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE);
+        liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    /*
+    To calculate power, we send the targetPosition value (how high we want the slider motors to
+    be based on motor 'ticks') to calculatePower function.
+
+    For every loop that occurs, it will constantly calculate power.  So long as we're far away from
+    our "target" position, we will get as much power as possible.  The closer we get, the lower the
+    power we will receive.
+    */
+    public void update() {
+        double power = calculatePower();
+        if(power < 0.1 && power > 0.05){
+            power = 0.12;
+        }
+        liftSlideLeft.setPower(power);
+        liftSlideRight.setPower(power);
+
+    }
+
+    /*
+    Calculating power - To calculate the power, we determine the proportion, derivative and
+    integral of our closed loop system.
+
+    For more information, please visit:
+    - Introduction to Closed Loop System:
+      - https://www.ctrlaltftc.com/introduction-to-closed-loop-control
+    - The PID controller:
+      - https://www.ctrlaltftc.com/the-pid-controller
+    */
+    private double calculatePower() {
+        double error = getTargetPosition() - liftSlideLeft.getCurrentPosition();
+        if(error < 20 && error > 0){
+            error = 20;
+        }else if(error > -20 && error < 0)
+        integralSum += error * timer.seconds();
+        double derivative = (error - lastError) / timer.seconds();
+        lastError = error;
+        timer.reset();
+        return (error * kp) + (derivative * kd) + (integralSum * ki);
+    }
+
+    public void toLowBucketPosition() {
+        setTargetPosition(liftToLowBucketPos);
+    }
+
+    public void toHighBucketPosition() {
+        setTargetPosition(liftToHighBucketPos);
+    }
+
+    public void toFixedPosition(int value) {
+        setTargetPosition(value);
+    }
+
+    /**
+     * go to a specified position (measure in CM)
+     * Measured from the top of the left Slide
+     *At the floor, measurement is 34 cm
+     * @param centimeters height you want to go to, lowest is 34
+     * UNFINISHED
+     */
+    public void toCentimeterMeasurement(int centimeters){
+        int ticksPerCm = 60;
+        int HeightWhenAtFloor = 34;
+        if(centimeters > 0) {
+            liftSlideLeft.setTargetPosition((centimeters+ 34)* ticksPerCm);
+            liftSlideRight.setTargetPosition((centimeters+ 34) * ticksPerCm);
+        }
+    }
+    public int getCM(){
+        int ticksPerCm = 60;
+        int HeightWhenAtFloor = 34;
+        return 34 +(liftSlideLeft.getCurrentPosition()/ticksPerCm);
+    }
+    public void toHangHeight(){setTargetPosition(slideHangBlueberrySkyhook);}
+    public void toHangBelowFloor(){
+        setTargetPosition(slideBelowFloor);
+
+    }
+
+    public void toSpecimanHangHeight(){
+        setTargetPosition(slideSpecimanHang);
+    }
+
+    public void toSpecimanReleaseHeight(){
+        setTargetPosition(slideSpecimanRelease);
+    }
+
+
+
+    public int getFixedPosition() {
+        return liftSlideLeft.getCurrentPosition();
+    }
+
+    public void toFloorPosition(){setTargetPosition(0);}
+    public void toHighBucketReverseDrop(){setTargetPosition(backwardBucketDrop);}
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java

@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.HOOK;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.toBar;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.toFloor;
+
+public class HangMotorSubsystem {
+    private DcMotorEx hang;
+
+    public HangMotorSubsystem(HardwareMap hardwareMap) {
+        hang = hardwareMap.get(DcMotorEx.class, HOOK);
+    }
+
+    public int getCurrentPosition(){
+        int Return = hang.getCurrentPosition();
+        return Return;
+    }
+    public void init(){
+        //init hook, set runmodes
+        //hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        hang.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+    }
+    public void hangRobot(){
+        hang.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        hang.setTargetPosition(toBar);
+        while(hang.isBusy()){
+            hang.setPower(1);
+        }
+    }
+    public void robotToFloor(){
+        hang.setTargetPosition(toFloor);
+    }
+    public void disableMotor(){
+        hang.setPower(0);
+
+    }
+    public void setPowerForward(double power){
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        hang.setPower(power);
+    }
+    public void setPowerReverse(double power){
+        hang.setDirection(DcMotorSimple.Direction.REVERSE);
+        hang.setPower(power);
+    }
+    public void stopMotor(){
+        hang.setPower(0);
+
+
+    }
+    public void toHangPosition(int Position){
+        //write in limits for protection
+        hang.setPower(Position);
+    }
+    public void setPower(boolean forward, double power){
+        if(forward)
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        else if(!forward)
+            hang.setDirection(DcMotorSimple.Direction.REVERSE);
+        hang.setPower(power);
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HookTest.java

@@ -0,0 +1,97 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp(name = "Hook Test", group = "Debug")
+public class HookTest extends OpMode {
+
+    private HangMotorSubsystem skyhook;
+    private ArmSubsystem arm;
+    private WristSubsystem wrist;
+    private ClawSubsystem claw;
+    private DualMotorSliderSubsystem slides;
+    @Override
+    public void init() {
+        skyhook = new HangMotorSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        slides = new DualMotorSliderSubsystem(hardwareMap);
+        claw.init();
+        arm.initTeleOp();
+        wrist.initTeleOp();
+        slides.init();
+        claw.thumbUp();
+    }
+
+    @Override
+    public void loop() {
+        telemetry.addData("hook", skyhook.getCurrentPosition());
+        telemetry.addData("arm", arm.getState());
+        telemetry.addData("wrist", wrist.getState());
+        telemetry.addData("slides", slides.getCurrentPosition());
+        telemetry.update();
+        slides.update();
+
+
+        if (gamepad1.a) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
+            arm.grabBlueberrySkyhook();
+            telemetry.update();
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+            telemetry.update();
+
+
+            }
+        if(gamepad1.b) {
+            //confirm grab
+            claw.closeClaw();
+
+        }
+
+        if (gamepad1.x) {
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+            slides.toFixedPosition(500);
+            slides.update();
+        }
+        if (gamepad1.y) {
+            arm.hangBlueberrySkyhook();
+            wrist.hangBlueberrySkyhook();
+            try {
+                Thread.sleep(500);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            slides.toFixedPosition(1800);
+            slides.update();
+        }
+        if (gamepad1.right_bumper) {
+            claw.openClaw();
+        }
+        if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
+            skyhook.hangRobot();
+        }
+        if (gamepad1.left_trigger > 0) {
+            skyhook.disableMotor();
+            skyhook.setPowerReverse(1.0);
+        }
+        else if(gamepad1.left_trigger == 0){
+            skyhook.setPowerReverse(0.0);
+        }
+        if (gamepad1.right_trigger > 0) {
+            skyhook.disableMotor();
+            skyhook.setPowerForward(1.0);
+        }
+        else if(gamepad1.right_trigger == 0){
+            skyhook.setPowerForward(0.0);
+        }
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Inspection.java

@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
+@TeleOp(name = "Inspection", group = "Debug")
+public class Inspection extends OpMode {
+    private DualMotorSliderSubsystem dualSlides;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+    private HangMotorSubsystem skyhook;
+
+    @Override
+    public void init() {
+        dualSlides = new DualMotorSliderSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        skyhook = new HangMotorSubsystem(hardwareMap);
+        dualSlides.init();
+        claw.init();
+        wrist.initTeleOp();
+        arm.initTeleOp();
+        skyhook.init();
+    }
+
+    @Override
+    public void loop() {
+        telemetry.addData("a: ", "init");
+        telemetry.addData("b: ", "armFloor/WristFloor");
+        telemetry.addData("y: ", "GrabSkyhook");
+        if(gamepad1.a){
+            wrist.InitAuto();
+            arm.initAuto();
+        }
+        if(gamepad1.b){
+            arm.toFloorPosition();
+            wrist.toFloorPositionTeleop();
+        }
+        if (gamepad1.y) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
+            arm.grabBlueberrySkyhook();
+
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LiftArmWrist.java

@@ -0,0 +1,128 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+/* Copyright (c) 2021 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.
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+
+@TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
+public class LiftArmWrist extends LinearOpMode {
+
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Lift
+         */
+        DualMotorSliderSubsystem lift = new DualMotorSliderSubsystem(hardwareMap);
+        WristSubsystem wrist = new WristSubsystem(hardwareMap);
+        ArmSubsystem arm = new ArmSubsystem(hardwareMap);
+        ClawSubsystem claw = new ClawSubsystem(hardwareMap);
+        HangMotorSubsystem hang = new HangMotorSubsystem(hardwareMap);
+
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        lift.init();
+        wrist.initTeleOp();
+        arm.initTeleOp();
+        claw.init();
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
+                arm.setPosition(arm.getPosition() + .05);
+            }
+
+            if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
+                arm.setPosition(arm.getPosition() - .05);
+            }
+
+            if (currentGamepad1.b && !previousGamepad1.b) {
+                claw.switchState();
+            }
+
+            if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
+                lift.toSpecimanHangHeight();
+            }
+
+            if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
+                lift.toSpecimanReleaseHeight();
+            }
+
+            if (currentGamepad1.y && !previousGamepad1.y) {
+                wrist.setPosition(wrist.getPosition() + .05);
+            }
+
+            if (currentGamepad1.a && !previousGamepad1.a) {
+                wrist.setPosition(wrist.getPosition() - .05);
+            }
+
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+               lift.setTargetPosition(lift.getCurrentPosition() + 250);
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                lift.setTargetPosition(lift.getCurrentPosition() - 250);
+
+            }
+
+
+            lift.update();
+
+            // Show the elapsed game time and wheel power.
+
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Lift Drive Position", lift.getFixedPosition());
+            telemetry.addData("Wrist Position", wrist.getPosition());
+            telemetry.addData("Arm Position", arm.getPosition());
+            telemetry.addData("Slide Ticks", lift.getCurrentPosition());
+            telemetry.addData("Hook Ticks", hang.getCurrentPosition());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MotorsSubsystem.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.subsystem;
+package org.firstinspires.ftc.teamcode.subsystems;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
@@ -10,12 +10,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/WristSubsystem.java

@@ -0,0 +1,138 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBackwardBucket;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenHang;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenPrep;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+
+public class WristSubsystem {
+
+    public enum WristState {
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET, TRAVEL
+    }
+
+    public ServoImplEx wrist;
+    public WristState state;
+
+    public WristSubsystem(HardwareMap hardwareMap) {
+        this.wrist = hardwareMap.get(ServoImplEx.class, WRIST_SERVO);
+    }
+
+    public class MoveToPosition implements Action {
+        private double positionValue;
+        private WristState positionState;
+
+        public MoveToPosition(double positionValue, WristState positionState) {
+            this.positionValue = positionValue;
+            this.positionState = positionState;
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+            setState(positionState);
+            wrist.setPosition(positionValue);
+            telemetryPacket.put("Wrist State", positionState);
+            return false;
+        }
+    }
+    public void grabBlueberrySkyhook(){
+        setState(WristState.GRAB_BLUEBERRY_SKYHOOK);
+        wrist.setPosition(wristGrabBlueberrySkyhook);
+    }
+
+
+    public void hangBlueberrySkyhook(){
+        setState(WristState.HANG_BLUEBERRY_SKYHOOK);
+        wrist.setPosition(wristHangBlueberrySkyhook);
+    }
+    public void toFloorPosition() {
+        setState(WristState.FLOOR);
+        wrist.setPosition(wristFloor);
+    }
+
+    public void toFloorPositionTeleop() {
+        setState(WristState.FLOOR);
+        wrist.setPosition(0.69);
+    }
+
+    public void toBucketPosition() {
+        setState(WristState.BUCKET);
+        wrist.setPosition(wristBucket);
+    }
+
+    public void toSpecimenPrep() {
+        setState(WristState.SPECIMEN);
+        wrist.setPosition(wristSpecimenPrep);
+    }
+
+    public void toSpecimenHang() {
+        setState(WristState.SPECIMEN);
+        wrist.setPosition(wristSpecimenHang);
+    }
+
+    public void toPickupPosition() {
+        setState(WristState.PICKUP);
+        wrist.setPosition(wristPickup);
+    }
+    public void toInitPosition(){
+        setState(WristState.INIT);
+        wrist.setPosition(wristInit);
+    }
+    public void InitAuto(){
+        wrist.resetDeviceConfigurationForOpMode();
+        toInitPosition();
+
+    }
+
+    public void setState(WristState wristState) {
+        this.state = wristState;
+    }
+
+    public void switchState() {
+        if (state == WristState.FLOOR) {
+            toPickupPosition();
+        } else if (state == WristState.PICKUP) {
+            toFloorPosition();
+        }
+    }
+    public void toReverseBucket(){
+        wrist.setPosition(wristBackwardBucket);
+        setState(WristState.REVERSE_BUCKET);
+    }
+    public void toTravelPosition(){
+        wrist.setPosition(wristtravel);
+        setState(WristState.TRAVEL);
+    }
+
+    public WristState getState() {
+        return this.state;
+    }
+
+    public void initTeleOp() {
+        wrist.resetDeviceConfigurationForOpMode();
+        toFloorPositionTeleop();
+    }
+
+    public void setPosition(double position) {
+        wrist.setPosition(position);
+    }
+
+    public double getPosition() {
+        return wrist.getPosition();
+    }
+
+}

build.common.gradle

@@ -21,7 +21,7 @@ apply plugin: 'com.android.application'
 
 android {
 
-    compileSdkVersion 29
+    compileSdkVersion 30
 
     signingConfigs {
         release {
@@ -113,10 +113,6 @@ android {
     packagingOptions {
         pickFirst '**/*.so'
     }
-    sourceSets.main {
-        jni.srcDirs = []
-        jniLibs.srcDir rootProject.file('libs')
-    }
     ndkVersion '21.3.6528147'
 }
 

build.gradle

@@ -5,13 +5,16 @@
  */
 
 buildscript {
+    ext {
+        agp_version = '8.1.4'
+    }
     repositories {
         mavenCentral()
         google()
     }
     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:$agp_version"
     }
 }
 

gradle.properties

@@ -8,3 +8,5 @@ android.enableJetifier=false
 
 # Allow Gradle to use up to 1 GB of RAM
 org.gradle.jvmargs=-Xmx1024M
+
+android.nonTransitiveRClass=false

gradle/wrapper/gradle-wrapper.properties

@@ -1,5 +1,6 @@
+#Thu Dec 26 18:45:39 PST 2024
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.1.1-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists