upgraded versions for sdk

FtcRobotController v12.0

FtcRobotController/src/main/AndroidManifest.xml

@@ -2,7 +2,7 @@
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
           xmlns:tools="http://schemas.android.com/tools"
     android:versionCode="63"
-          android:versionName="11.2.1">
+          android:versionName="12.0">
 
   <uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />
 

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

@@ -38,8 +38,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCompatibil
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.internal.usb.UsbConstants;
 import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection;
 
 import java.util.List;
 
@@ -150,12 +152,12 @@ public class ConceptAprilTag extends LinearOpMode {
         aprilTag = new AprilTagProcessor.Builder()
 
             // The following default settings are available to un-comment and edit as needed.
-            //.setDrawAxes(false)
-            //.setDrawCubeProjection(false)
+            //.setDrawAxes(true) // Changed in V12.0
             //.setDrawTagOutline(true)
             //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
             //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary())
             //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
+            //.setDrawCubeProjection(false)
 
             // == CAMERA CALIBRATION ==
             // If you do not manually specify calibration parameters, the SDK will attempt
@@ -220,14 +222,25 @@ public class ConceptAprilTag extends LinearOpMode {
 
         // Step through the list of detections and display info for each one.
         for (AprilTagDetection detection : currentDetections) {
-            if (detection.metadata != null) {
-                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
+            if (detection instanceof AprilTagSingleDetection) {
+                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+
+                if (singleDet.metadata != null) {
+                    telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name));
+                    telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
+                    telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
+                    telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
+                } else {
+                    telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id));
+                    telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", singleDet.center.x, singleDet.center.y));
+                }
+            } else {
+                AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection;
+                telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name));
+                telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound));
                 telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
                 telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
                 telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
-            } else {
-                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
-                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
             }
         }   // end for() loop
 

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

@@ -36,8 +36,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDir
 import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCompatibilityManager;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection;
 
 import java.util.List;
 
@@ -45,6 +47,8 @@ import java.util.List;
  * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using
  * the easy way.
  *
+ * Note: See ConceptAprilTag.java for how to add a camera compatibility quirk
+ *
  * For an introduction to AprilTags, see the FTC-DOCS link below:
  * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
  *
@@ -78,33 +82,10 @@ public class ConceptAprilTagEasy extends LinearOpMode {
      */
     private VisionPortal visionPortal;
 
-    // To find the VID/PID for a camera:
-    //
-    // Linux: open a terminal, run "lsusb", locate the line for your camera,
-    // and find the section that resembles "ID 1d6b:0002"; this is VID:PID
-    //
-    // OSX: open a terminal, run "system_profiler SPUSBDataType", locate the
-    // section for your camera, and find the "Product ID:" and "Vendor ID:"
-    // listings in the output
-    //
-    // Windows: open a PowerShell, run:
-    // Get-PnpDevice -PresentOnly | Where-Object { $_.InstanceId -like 'USB*' } | Select-Object FriendlyName, InstanceId
-    // and locate the line for your camera. The VID and PID is listed directly in the line.
-    static final int VENDOR_ID_SUNPLUS_INNOVATION_TECHNOLOGY = 0x1BCF;
-    static final int PRODUCT_ID_ARDUCAM_OV5648 = 0x284C;
-
     @Override
     public void runOpMode() {
 
-        // Demonstrate how to add a camera compatibility quirk
-        // these can sometimes be needed if a camera behaves poorly.
-        // Quirks have no effect unless the camera you are using matches the specified VID/PID
-        CameraCompatibilityManager.getInstance()
-                .addQuirk(
-                        VENDOR_ID_SUNPLUS_INNOVATION_TECHNOLOGY,
-                        PRODUCT_ID_ARDUCAM_OV5648,
-                        CameraCompatibilityManager.Quirk.AVOID_LIB_USB_RESET_DEVICE);
-
+        // See ConceptAprilTag.java for how to add a camera compatibility quirk
         initAprilTag();
 
         // Wait for the DS start button to be touched.
@@ -165,14 +146,25 @@ public class ConceptAprilTagEasy extends LinearOpMode {
 
         // Step through the list of detections and display info for each one.
         for (AprilTagDetection detection : currentDetections) {
-            if (detection.metadata != null) {
-                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
+            if (detection instanceof AprilTagSingleDetection) {
+                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+
+                if (singleDet.metadata != null) {
+                    telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name));
+                    telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
+                    telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
+                    telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
+                } else {
+                    telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id));
+                    telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", singleDet.center.x, singleDet.center.y));
+                }
+            }  else {
+                AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection;
+                telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name));
+                telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound));
                 telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
                 telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
                 telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
-            } else {
-                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
-                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
             }
         }   // end for() loop
 

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

@@ -43,6 +43,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
 import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection;
 
 import java.util.List;
 
@@ -177,7 +178,7 @@ public class ConceptAprilTagLocalization extends LinearOpMode {
         aprilTag = new AprilTagProcessor.Builder()
 
                 // The following default settings are available to un-comment and edit as needed.
-                //.setDrawAxes(false)
+                //.setDrawAxes(true)  // changed in V12.0
                 //.setDrawCubeProjection(false)
                 //.setDrawTagOutline(true)
                 //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
@@ -247,22 +248,23 @@ public class ConceptAprilTagLocalization extends LinearOpMode {
 
         // Step through the list of detections and display info for each one.
         for (AprilTagDetection detection : currentDetections) {
-            if (detection.metadata != null) {
-                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
-                // Only use tags that don't have Obelisk in them
-                if (!detection.metadata.name.contains("Obelisk")) {
-                    telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)",
+            if (detection instanceof AprilTagSingleDetection) {
+                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+
+                if (singleDet.metadata != null) {
+                    telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name));
+                    telemetry.addLine(String.format("Robot XYZ %6.1f %6.1f %6.1f  (inch)",
                             detection.robotPose.getPosition().x,
                             detection.robotPose.getPosition().y,
                             detection.robotPose.getPosition().z));
-                    telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)",
+                    telemetry.addLine(String.format("Robot PRY %6.1f %6.1f %6.1f  (deg)",
                             detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES),
                             detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES),
                             detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES)));
+                } else {
+                    telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id));
+                    telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", singleDet.center.x, singleDet.center.y));
                 }
-            } else {
-                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
-                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
             }
         }   // end for() loop
 

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

@@ -37,8 +37,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.VisionPortal.CameraState;
+import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection;
 
 import java.util.List;
 
@@ -153,14 +155,25 @@ public class ConceptAprilTagSwitchableCameras extends LinearOpMode {
 
         // Step through the list of detections and display info for each one.
         for (AprilTagDetection detection : currentDetections) {
-            if (detection.metadata != null) {
-                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
+            if (detection instanceof AprilTagSingleDetection) {
+                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+
+                if (singleDet.metadata != null) {
+                    telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name));
+                    telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
+                    telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
+                    telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
+                } else {
+                    telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id));
+                    telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", singleDet.center.x, singleDet.center.y));
+                }
+            }  else {
+                AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection;
+                telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name));
+                telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound));
                 telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
                 telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
                 telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
-            } else {
-                telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
-                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
             }
         }   // end for() loop
 

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

@@ -39,25 +39,32 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
 import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection;
 
 import java.util.List;
 import java.util.concurrent.TimeUnit;
 
 /*
- * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag or AprilTag Cluster
+ * A "Cluster" is a group of Apriltags that share a common origin, and are identified by name.
  * The code assumes a Holonomic (Mecanum or X Drive) Robot.
  *
  * For an introduction to AprilTags, see the ftc-docs link below:
  * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
  *
- * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
+ * When an AprilTag/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera.
  * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
  * https://ftc-docs.firstinspires.org/apriltag-detection-values
  *
- * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and
- * driving towards the tag to achieve the desired distance.
+ * For a single tag, the "Drive Target" is the center of the Tag.  For a cluster of tags, the "Drive Target" will be the
+ * (0,0,0) ORIGIN of the cluster, which may have been positioned somewhere other than the center of the cluster in order
+ * to help to locate a game objective.
+ *
+ * The driving goal is to rotate to keep the Target centered in the camera, while strafing to be directly in front of the target,
+ *  and driving towards the target to achieve the desired distance.
  * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
  * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
  *
@@ -73,9 +80,9 @@ import java.util.concurrent.TimeUnit;
  * Release the Left Bumper to return to manual driving mode.
  *
  * Under "Drive To Target" mode, the robot has three goals:
- * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
- * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front  of the tag.  (Use the Target Yaw to strafe the robot)
- * 3) Drive towards the Tag to get to the desired distance.  (Use Tag Range to drive the robot forward/backward)
+ * 1) Turn the robot to always keep the Target centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Strafe the robot towards the centerline of the Target, so it approaches directly in front  of the tag.  (Use the Target Yaw to strafe the robot)
+ * 3) Drive towards the Target to get to the desired distance.  (Use TargetRange to drive the robot forward/backward)
  *
  * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
  * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants.
@@ -90,7 +97,7 @@ import java.util.concurrent.TimeUnit;
 public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
 {
     // Adjust these numbers to suit your robot.
-    final double DESIRED_DISTANCE = 12.0; //  this is how close the camera should get to the target (inches)
+    final double DESIRED_DISTANCE = 30.0; //  this is how close the camera should get to the target (inches)
 
     //  Set the GAIN constants to control the relationship between the measured position error, and how much power is
     //  applied to the drive motors to correct the error.
@@ -99,24 +106,31 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
     final double STRAFE_GAIN =  0.015 ;   //  Strafe Speed Control "Gain".  e.g. Ramp up to 37% power at a 25 degree Yaw error.   (0.375 / 25.0)
     final double TURN_GAIN   =  0.01  ;   //  Turn Control "Gain".  e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0)
 
-    final double MAX_AUTO_SPEED = 0.5;   //  Clip the approach speed to this max value (adjust for your robot)
-    final double MAX_AUTO_STRAFE= 0.5;   //  Clip the strafing speed to this max value (adjust for your robot)
-    final double MAX_AUTO_TURN  = 0.3;   //  Clip the turn speed to this max value (adjust for your robot)
+    final double MAX_AUTO_SPEED = 0.5;    //  Clip the approach speed to this max value (adjust for your robot)
+    final double MAX_AUTO_STRAFE= 0.5;    //  Clip the strafing speed to this max value (adjust for your robot)
+    final double MAX_AUTO_TURN  = 0.3;    //  Clip the turn speed to this max value (adjust for your robot)
 
     private DcMotor frontLeftDrive = null;  //  Used to control the left front drive wheel
-    private DcMotor frontRightDrive = null;  //  Used to control the right front drive wheel
-    private DcMotor backLeftDrive = null;  //  Used to control the left back drive wheel
+    private DcMotor frontRightDrive = null; //  Used to control the right front drive wheel
+    private DcMotor backLeftDrive = null;   //  Used to control the left back drive wheel
     private DcMotor backRightDrive = null;  //  Used to control the right back drive wheel
 
-    private static final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
-    private static final int DESIRED_TAG_ID = -1;     // Choose the tag you want to approach or set to -1 for ANY tag.
+    private final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
+    private final int DESIRED_TAG_ID = -1;    // The tag you want to approach, or set to -1 for ANY tag.
+    private final String DESIRED_CLUSTER_NAME = null; // The cluster name you want to approach, or set null for ANY cluster.
+
     private VisionPortal visionPortal;               // Used to manage the video source.
     private AprilTagProcessor aprilTag;              // Used for managing the AprilTag detection process.
-    private AprilTagDetection desiredTag = null;     // Used to hold the data for a detected AprilTag
+
+    private boolean targetFound  = false;    // Set to true when an AprilTag/Cluster target is detected
+    private String targetName    = "none";
+    private int    targetID      = 0;
+    private double targetRange   = 0;
+    private double targetBearing = 0;
+    private double targetYaw     = 0;
 
     @Override public void runOpMode()
     {
-        boolean targetFound     = false;    // Set to true when an AprilTag target is detected
         double  drive           = 0;        // Desired forward power/speed (-1 to +1)
         double  strafe          = 0;        // Desired strafe power/speed (-1 to +1)
         double  turn            = 0;        // Desired turning power/speed (-1 to +1)
@@ -152,36 +166,57 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
         while (opModeIsActive())
         {
             targetFound = false;
-            desiredTag  = null;
 
             // Step through the list of detected tags and look for a matching tag
             List<AprilTagDetection> currentDetections = aprilTag.getDetections();
             for (AprilTagDetection detection : currentDetections) {
-                // Look to see if we have size info on this tag.
-                if (detection.metadata != null) {
-                    //  Check to see if we want to track towards this tag.
-                    if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
-                        // Yes, we want to use this tag.
-                        targetFound = true;
-                        desiredTag = detection;
-                        break;  // don't look any further.
+
+                if (detection instanceof AprilTagSingleDetection) {
+                    AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection;
+
+                    // Look to see if we have size info on this tag.
+                    if (singleDetection.metadata != null) {
+                        //  Check to see if we want to track towards this tag.
+                        if ((DESIRED_TAG_ID < 0) || (singleDetection.id == DESIRED_TAG_ID)) {
+                            // Yes, we want to use this tag.
+                            targetName    = singleDetection.metadata.name;
+                            targetID      = singleDetection.id;
+                            targetRange   = singleDetection.ftcPose.range;
+                            targetBearing = singleDetection.ftcPose.bearing;
+                            targetYaw     = singleDetection.ftcPose.yaw;
+                            targetFound = true;
+                            break;  // don't look any further.
+                        } else {
+                            // This tag is in the library, but we do not want to track it right now.
+                            telemetry.addData("Skipping", "Tag ID %d is not desired", singleDetection.id);
+                        }
                     } else {
-                        // This tag is in the library, but we do not want to track it right now.
-                        telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+                        // This tag is NOT in the library, so we don't have enough information to track to it.
+                        telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", singleDetection.id);
                     }
                 } else {
-                    // This tag is NOT in the library, so we don't have enough information to track to it.
-                    telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+                    AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection;
+
+                    if (DESIRED_CLUSTER_NAME == null || clusterDet.metadata.shortName.equals(DESIRED_CLUSTER_NAME) ) {
+                        // Yes, we want to use this tag.
+                        targetName    = clusterDet.metadata.shortName;
+                        targetID      = -1;
+                        targetRange   = clusterDet.ftcPose.range;
+                        targetBearing = clusterDet.ftcPose.bearing;
+                        targetYaw     = clusterDet.ftcPose.yaw;
+                        targetFound = true;
+                        break;  // don't look any further.
+                    }
                 }
             }
 
             // Tell the driver what we see, and what to do.
             if (targetFound) {
                 telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
-                telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
-                telemetry.addData("Range",  "%5.1f inches", desiredTag.ftcPose.range);
-                telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
-                telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
+                telemetry.addData("Found", "ID %d (%s)", targetID, targetName);
+                telemetry.addData("Range",  "%5.1f inches", targetRange);
+                telemetry.addData("Bearing","%3.0f degrees", targetBearing);
+                telemetry.addData("Yaw","%3.0f degrees", targetYaw);
             } else {
                 telemetry.addData("\n>","Drive using joysticks to find valid target\n");
             }
@@ -190,9 +225,9 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
             if (gamepad1.left_bumper && targetFound) {
 
                 // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically.
-                double  rangeError      = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
-                double  headingError    = desiredTag.ftcPose.bearing;
-                double  yawError        = desiredTag.ftcPose.yaw;
+                double  rangeError      = targetRange - DESIRED_DISTANCE;
+                double  headingError    = targetBearing;
+                double  yawError        = targetYaw;
 
                 // Use the speed and turn "gains" to calculate how we want the robot to move.
                 drive  = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
@@ -201,7 +236,6 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
 
                 telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn);
             } else {
-
                 // drive using manual POV Joystick mode.  Slow things down to make the robot more controlable.
                 drive  = -gamepad1.left_stick_y  / 2.0;  // Reduce drive rate to 50%.
                 strafe = -gamepad1.left_stick_x  / 2.0;  // Reduce strafe rate to 50%.
@@ -218,11 +252,8 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
 
     /**
      * Move robot according to desired axes motions
-     * <p>
      * Positive X is forward
-     * <p>
      * Positive Y is strafe left
-     * <p>
      * Positive Yaw is counter-clockwise
      */
     public void moveRobot(double x, double y, double yaw) {

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

@@ -39,44 +39,50 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
 import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection;
 
 import java.util.List;
 import java.util.concurrent.TimeUnit;
 
 /*
- * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
- * The code assumes a basic two-wheel (Tank) Robot Drivetrain
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag or AprilTag Cluster
+ * A "Cluster" is a group of Apriltags that share a common origin, and are identified by name.
+ * The code assumes a basic two-motor Tank (differential) drive robot.
  *
  * For an introduction to AprilTags, see the ftc-docs link below:
  * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
  *
- * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
+ * When an AprilTag/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera.
  * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
  * https://ftc-docs.firstinspires.org/apriltag-detection-values
  *
- * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance.
- * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
- * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ * For a single tag, the "Drive Target" is the center of the Tag.  For a cluster of tags, the "Drive Target" will be the
+ * (0,0,0) ORIGIN of the cluster, which may have been positioned somewhere other than the center of the cluster in order
+ * to help to locate a game objective.
  *
- * The code assumes a Robot Configuration with motors named left_drive and right_drive.
- * The motor directions must be set so a positive power goes forward on both wheels;
- * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default
+ * The driving goal is to rotate to keep the Target centered in the camera, while driving towards the target to achieve the desired distance.
+ * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
+ * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ *
+ * The code assumes a Robot Configuration with motors named: left_drive and right_drive.
+ * The motor directions must be set so a positive power goes forward on all wheels.
+ * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default,
  * so you should choose to approach a valid tag ID.
  *
- * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot.
- * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel).
- *
+ * Under manual control, the left stick will move forward/back & left/right.  The right stick will rotate the robot.
  * Manually drive the robot until it displays Target data on the Driver Station.
+ *
  * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
  * Release the Left Bumper to return to manual driving mode.
  *
- *  Under "Drive To Target" mode, the robot has two goals:
- *  1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
- *  2) Drive towards the Tag to get to the desired distance.  (Use Tag Range to drive the robot forward/backward)
+ * Under "Drive To Target" mode, the robot has two goals:
+ * 1) Turn the robot to always keep the Target centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Drive towards the Target to get to the desired distance.  (Use TargetRange to drive the robot forward/backward)
  *
- *  Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
+ * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
  * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants.
  *
  * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder.
@@ -89,7 +95,7 @@ import java.util.concurrent.TimeUnit;
 public class RobotAutoDriveToAprilTagTank extends LinearOpMode
 {
     // Adjust these numbers to suit your robot.
-    final double DESIRED_DISTANCE = 12.0; //  this is how close the camera should get to the target (inches)
+    final double DESIRED_DISTANCE = 30.0; //  this is how close the camera should get to the target (inches)
 
     //  Set the GAIN constants to control the relationship between the measured position error, and how much power is
     //  applied to the drive motors to correct the error.
@@ -103,15 +109,22 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
     private DcMotor leftDrive   = null;  //  Used to control the left drive wheel
     private DcMotor rightDrive  = null;  //  Used to control the right drive wheel
 
-    private static final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
-    private static final int DESIRED_TAG_ID = -1;    // Choose the tag you want to approach or set to -1 for ANY tag.
+    private final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
+    private final int DESIRED_TAG_ID = -1;    // The tag you want to approach, or set to -1 for ANY tag.
+    private final String DESIRED_CLUSTER_NAME = null; // The cluster name you want to approach, or set null for ANY cluster.
+
     private VisionPortal visionPortal;               // Used to manage the video source.
     private AprilTagProcessor aprilTag;              // Used for managing the AprilTag detection process.
-    private AprilTagDetection desiredTag = null;     // Used to hold the data for a detected AprilTag
+
+    private boolean targetFound  = false;    // Set to true when an AprilTag/Cluster target is detected
+    private String targetName    = "none";
+    private int    targetID      = 0;
+    private double targetRange   = 0;
+    private double targetBearing = 0;
+    private double targetYaw     = 0;
 
     @Override public void runOpMode()
     {
-        boolean targetFound     = false;    // Set to true when an AprilTag target is detected
         double  drive           = 0;        // Desired forward power/speed (-1 to +1) +ve is forward
         double  turn            = 0;        // Desired turning power/speed (-1 to +1) +ve is CounterClockwise
 
@@ -142,35 +155,57 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
         while (opModeIsActive())
         {
             targetFound = false;
-            desiredTag  = null;
 
             // Step through the list of detected tags and look for a matching tag
             List<AprilTagDetection> currentDetections = aprilTag.getDetections();
             for (AprilTagDetection detection : currentDetections) {
-                // Look to see if we have size info on this tag.
-                if (detection.metadata != null) {
-                    //  Check to see if we want to track towards this tag.
-                    if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
-                        // Yes, we want to use this tag.
-                        targetFound = true;
-                        desiredTag = detection;
-                        break;  // don't look any further.
+
+                if (detection instanceof AprilTagSingleDetection) {
+                    AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection;
+
+                    // Look to see if we have size info on this tag.
+                    if (singleDetection.metadata != null) {
+                        //  Check to see if we want to track towards this tag.
+                        if ((DESIRED_TAG_ID < 0) || (singleDetection.id == DESIRED_TAG_ID)) {
+                            // Yes, we want to use this tag.
+                            targetName    = singleDetection.metadata.name;
+                            targetID      = singleDetection.id;
+                            targetRange   = singleDetection.ftcPose.range;
+                            targetBearing = singleDetection.ftcPose.bearing;
+                            targetYaw     = singleDetection.ftcPose.yaw;
+                            targetFound = true;
+                            break;  // don't look any further.
+                        } else {
+                            // This tag is in the library, but we do not want to track it right now.
+                            telemetry.addData("Skipping", "Tag ID %d is not desired", singleDetection.id);
+                        }
                     } else {
-                        // This tag is in the library, but we do not want to track it right now.
-                        telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+                        // This tag is NOT in the library, so we don't have enough information to track to it.
+                        telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", singleDetection.id);
+                    }
+                }  else {
+                    AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection;
+
+                    if (DESIRED_CLUSTER_NAME == null || clusterDet.metadata.shortName.equals(DESIRED_CLUSTER_NAME) ) {
+                        // Yes, we want to use this tag.
+                        targetName    = clusterDet.metadata.shortName;
+                        targetID      = -1;
+                        targetRange   = clusterDet.ftcPose.range;
+                        targetBearing = clusterDet.ftcPose.bearing;
+                        targetYaw     = clusterDet.ftcPose.yaw;
+                        targetFound = true;
+                        break;  // don't look any further.
                     }
-                } else {
-                    // This tag is NOT in the library, so we don't have enough information to track to it.
-                    telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
                 }
             }
 
             // Tell the driver what we see, and what to do.
             if (targetFound) {
                 telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
-                telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
-                telemetry.addData("Range",  "%5.1f inches", desiredTag.ftcPose.range);
-                telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
+                telemetry.addData("Found", "ID %d (%s)", targetID, targetName);
+                telemetry.addData("Range",  "%5.1f inches", targetRange);
+                telemetry.addData("Bearing","%3.0f degrees", targetBearing);
+                telemetry.addData("Yaw","%3.0f degrees", targetYaw);
             } else {
                 telemetry.addData("\n>","Drive using joysticks to find valid target\n");
             }
@@ -179,8 +214,8 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
             if (gamepad1.left_bumper && targetFound) {
 
                 // Determine heading and range error so we can use them to control the robot automatically.
-                double  rangeError   = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
-                double  headingError = desiredTag.ftcPose.bearing;
+                double  rangeError      = targetRange - DESIRED_DISTANCE;
+                double  headingError    = targetBearing;
 
                 // Use the speed and turn "gains" to calculate how we want the robot to move.  Clip it to the maximum
                 drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
@@ -188,7 +223,6 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
 
                 telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn);
             } else {
-
                 // drive using manual POV Joystick mode.
                 drive = -gamepad1.left_stick_y  / 2.0;  // Reduce drive rate to 50%.
                 turn  = -gamepad1.right_stick_x / 4.0;  // Reduce turn rate to 25%.
@@ -204,9 +238,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
 
     /**
      * Move robot according to desired axes motions
-     * <p>
      * Positive X is forward
-     * <p>
      * Positive Yaw is counter-clockwise
      */
     public void moveRobot(double x, double yaw) {

README.md

@@ -1 +1 @@
-# BioBuzz FTC Robot Code for 2026-2027 Season
+# BioBuzz FTC Robot Code for Panther Robotics

TeamCode/build.gradle

@@ -24,6 +24,9 @@ android {
     }
 }
 
+
 dependencies {
     implementation project(':FtcRobotController')
+
+    implementation "org.solverslib:core:0.3.5" // core
 }

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

@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.hardware.RobotHardware;
+
+/**
+ * Represents and initializes the complete robot object with subsystems.
+ */
+public final class Robot {
+    public final RobotHardware robotHardware;
+
+    // Declare other public final subsystems here
+
+    public Robot(HardwareMap map) {
+
+        robotHardware = new RobotHardware(map);
+
+        // Initialize other subsystems using robotHardware reference
+
+    }
+}

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

@@ -0,0 +1,14 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+/**
+ * Initializes and provides access to all the robot's hardware components
+ */
+public final class RobotHardware {
+
+    // Declare public final motors, servos, etc.
+    public RobotHardware(HardwareMap map) {
+        // Initialize motors, servos, etc.
+    }
+}

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

@@ -0,0 +1,11 @@
+package org.firstinspires.ftc.teamcode.pedro;
+
+import com.pedropathing.follower.Follower;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class Constants {
+    public static Follower create(HardwareMap h) {
+        // return new Follower(Drivetrain, Localizer, Foresight);
+        return null;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java

@@ -0,0 +1,5 @@
+package org.firstinspires.ftc.teamcode.pedro;
+
+public class Tuning {
+    // Tuners go here
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/MecanumTuner.java

@@ -0,0 +1,96 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.tuning.autotune.*;
+import com.pedropathing.tuning.autotune.Display.FourWheelBot.Wheel;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+enum Direction {
+    @DisplayName("Forward") FORWARD,
+    @DisplayName("Reversed") REVERSE
+}
+
+public class MecanumTuner extends Procedure {
+    public MecanumTuner() {
+        super("Mecanum Tuner", "A procedure to find the directions of mecanum wheels.");
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        Inputs motorNames = inputs("Mecanum Motor Names", "Enter the names in HardwareMap of your drivetrain motors.");
+        Inputs.Field<String> frontLeftName = motorNames.s("Front Left Name");
+        Inputs.Field<String> frontRightName = motorNames.s("Front Right Name");
+        Inputs.Field<String> backLeftName = motorNames.s("Back Left Name");
+        Inputs.Field<String> backRightName = motorNames.s("Back Right Name");
+        awaitInputs(motorNames);
+
+        confirmation("Motor Directions", "Each drivetrain motor will spin, one at a time. After each one, you will enter whether it spun forward or reversed. You may use the interactive diagram to see which wheel should be spinning and which direction is forward.");
+
+        Direction frontLeftDirection = testMotor(Wheel.FRONT_LEFT, "Front Left", frontLeftName.get());
+        Direction frontRightDirection = testMotor(Wheel.FRONT_RIGHT, "Front Right", frontRightName.get());
+        Direction backLeftDirection = testMotor(Wheel.BACK_LEFT, "Back Left", backLeftName.get());
+        Direction backRightDirection = testMotor(Wheel.BACK_RIGHT, "Back Right", backRightName.get());
+
+        result("frontLeftName", frontLeftName.get());
+        result("frontRightName", frontRightName.get());
+        result("backLeftName", backLeftName.get());
+        result("backRightName", backRightName.get());
+        result("frontLeftDirection", frontLeftDirection);
+        result("frontRightDirection", frontRightDirection);
+        result("backLeftDirection", backLeftDirection);
+        result("backRightDirection", backRightDirection);
+
+        code(Language.JAVA, "public static MecanumConfig drivetrainConfig = new MecanumConfig(c -> {\n" +
+                "    c.frontLeftName.set(\"" + frontLeftName.get() + "\");\n" +
+                "    c.frontRightName.set(\"" + frontRightName.get() + "\");\n" +
+                "    c.backLeftName.set(\"" + backLeftName.get() + "\");\n" +
+                "    c.backRightName.set(\"" + backRightName.get() + "\");\n" +
+                "    c.frontLeftDirection.set(DcMotorSimple.Direction." + frontLeftDirection + ");\n" +
+                "    c.frontRightDirection.set(DcMotorSimple.Direction." + frontRightDirection + ");\n" +
+                "    c.backLeftDirection.set(DcMotorSimple.Direction." + backLeftDirection + ");\n" +
+                "    c.backRightDirection.set(DcMotorSimple.Direction." + backRightDirection + ");\n" +
+                "});");
+    }
+
+    private Direction testMotor(Wheel wheel, String displayName, String hardwareName) throws InterruptedException {
+        final boolean[] correctMotor = new boolean[1];
+        final Direction[] direction = new Direction[1];
+
+        withDisplay(Display.fourWheelBot(wheel, false), () -> {
+            runOpMode(new SpinMotor(displayName, hardwareName));
+
+            Inputs inputs = inputs(displayName, "Determine the " + displayName.toLowerCase() + " motor direction.");
+            Inputs.Field<Boolean> correctMotorField = inputs.b("Did the " + displayName.toLowerCase() + " motor spin?").withDefault(true);
+            Inputs.Field<Direction> directionField = inputs.e("Which way did the motor spin?", Direction.class);
+            awaitInputs(inputs);
+
+            correctMotor[0] = correctMotorField.get();
+            direction[0] = directionField.get();
+        });
+
+        if (!correctMotor[0])
+            abort("The wrong motor spun. Check that your motors are plugged into the correct ports, and that they are configured correctly. Then, try again.");
+
+        return direction[0];
+    }
+}
+
+class SpinMotor extends TuningOpMode<Void> {
+    private final String name;
+
+    public SpinMotor(String displayName, String hardwareName) {
+        super(displayName, "The " + displayName.toLowerCase() + " motor will spin. The interactive diagram shows which way is forward. Click stop when you know if it is spinning forward or reversed.", true);
+        this.name = hardwareName;
+    }
+
+    @SuppressWarnings("StatementWithEmptyBody")
+    @Override
+    protected Void runTuningOpMode() {
+        DcMotor motor = hardwareMap.dcMotor.get(name);
+        waitForStart();
+        motor.setPower(0.5);
+        while (opModeIsActive()) {
+        }
+        motor.setPower(0);
+        return null;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OTOSTuner.java

@@ -0,0 +1,210 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.math.Pose;
+import com.pedropathing.revhub.localizers.OTOSConfig;
+import com.pedropathing.revhub.localizers.OTOSLocalizer;
+import com.pedropathing.tuning.autotune.Inputs;
+import com.pedropathing.tuning.autotune.Procedure;
+import com.pedropathing.tuning.autotune.TuningOpMode;
+import com.pedropathing.utils.Angle;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+import java.util.List;
+
+public class OTOSTuner extends Procedure {
+    public OTOSTuner() {
+        super("OTOS Tuner", "A procedure for tuning the OTOS localizer.");
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        Inputs inputs = inputs("Setup", "Set OTOS HardwareMap Name");
+        Inputs.Field<String> name = inputs.s("HardwareMap Name").withDefault("otos");
+        awaitInputs(inputs);
+
+        Inputs scalar = inputs(
+                "Scalar Identification",
+                "Set the distance you will push your robot forward in inches and the number of full rotations for the angular test"
+        );
+        Inputs.Field<Double> distance = scalar.d("Distance to push robot").withDefault(48.0);
+        Inputs.Field<Integer> turns = scalar.i("Full rotations").withDefault(10);
+        awaitInputs(scalar);
+
+        if (!(distance.get() > 0.0)) {
+            abort("Enter a positive push distance in inches.");
+            return;
+        }
+        if (turns.get() <= 0) {
+            abort("Enter a positive number of full rotations.");
+            return;
+        }
+
+        Double angularScalar = runOpMode(new OTOSAngularScalar(name.get(), turns.get()));
+        Double linearScalar = runOpMode(new OTOSLinearScalar(name.get(), distance.get()));
+
+        List<Double> offsets = runOpMode(new OTOSOffsets(name.get(), linearScalar, angularScalar));
+        if (offsets == null) {
+            abort("Offset stage ended without a saved pose. Rotate the robot 180 degrees about the robot center, then press Stop.");
+            return;
+        }
+
+        result("name", name.get());
+        result("linearScalar", linearScalar);
+        result("angularScalar", angularScalar);
+        result("xOffset", offsets.get(0));
+        result("yOffset", offsets.get(1));
+
+        code(Language.JAVA,"public static OTOSConfig localizerConfig = new OTOSConfig(c -> {\n" +
+                "    c.name.set(\"" + name.get() + "\");\n" +
+                "    c.linearScalar.set(" + linearScalar + ");\n" +
+                "    c.angularScalar.set(" + angularScalar + ");\n" +
+                "    c.offset.set(new Pose(" + offsets.get(0) + ", " + offsets.get(1) + "));\n" +
+                "    c.linearUnit.set(DistanceUnit.INCH);\n" +
+                "});");
+    }
+
+}
+
+class OTOSLinearScalar extends TuningOpMode<Double> {
+    String name;
+    double distance;
+
+    public OTOSLinearScalar(String name, double distance) {
+        super("Linear Scalar Identification",
+                "Determines the linear scalar for the OTOS localizer. \n"
+                        + "Push your robot forward " + distance + " inches, stop moving, then press Stop",
+                true);
+        this.name = name;
+        this.distance = distance;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        OTOSConfig config = new OTOSConfig(c -> {
+            c.name.set(name);
+            c.linearUnit.set(DistanceUnit.INCH);
+            c.linearScalar.set(1.0);
+            c.angularScalar.set(1.0);
+            c.offset.set(Pose.zero());
+        });
+        OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config);
+        localizer.setPose(Pose.zero());
+        localizer.update();
+
+        Pose position = null;
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+            position = localizer.pose();
+
+
+        }
+
+        if (position == null || Math.abs(position.x()) <= 1e-9) {
+            return null;
+        }
+
+        return Math.abs(distance / position.x());
+    }
+}
+
+class OTOSAngularScalar extends TuningOpMode<Double> {
+    String name;
+    int turns;
+    double targetRadians;
+
+    public OTOSAngularScalar(String name, int turns) {
+        super("Angular Scalar Identification",
+                "Determines the angular scalar for the OTOS localizer. \n"
+                        + "Spin your robot " + turns + " full rotations, stop moving, then press Stop",
+                true);
+        this.name = name;
+        this.turns = turns;
+        this.targetRadians = turns * 2.0 * Math.PI;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        OTOSConfig config = new OTOSConfig(c -> {
+            c.name.set(name);
+            c.linearUnit.set(DistanceUnit.INCH);
+            c.linearScalar.set(1.0);
+            c.angularScalar.set(1.0);
+            c.offset.set(Pose.zero());
+        });
+        OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config);
+        localizer.setPose(Pose.zero());
+        localizer.update();
+
+        waitForStart();
+
+        localizer.update();
+        double prevHeading = localizer.pose().heading();
+        double totalHeading = 0.0;
+
+        while (!isStopRequested()) {
+            localizer.update();
+
+            Pose position = localizer.pose();
+            double currentHeading = position.heading();
+            totalHeading += Angle.normalizeSigned(currentHeading - prevHeading);
+            prevHeading = currentHeading;
+
+
+        }
+
+        if (Math.abs(totalHeading) <= 1e-9) {
+            return null;
+        }
+
+        return Math.abs(targetRadians / totalHeading);
+    }
+}
+
+class OTOSOffsets extends TuningOpMode<List<Double>> {
+    String name;
+    double linearScalar, angularScalar;
+
+    public OTOSOffsets(String name, double linearScalar, double angularScalar) {
+        super("OTOS Offset Identification",
+                "Automatically identifies the X/Y offset for your OTOS localizer. \n"
+                        + "Rotate the robot 180 degrees counterclockwise about the robot center without translating it, stop moving, then press Stop",
+                true);
+        this.name = name;
+        this.linearScalar = linearScalar;
+        this.angularScalar = angularScalar;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        OTOSConfig config = new OTOSConfig(c -> {
+            c.name.set(name);
+            c.linearUnit.set(DistanceUnit.INCH);
+            c.linearScalar.set(linearScalar);
+            c.angularScalar.set(angularScalar);
+            c.offset.set(Pose.zero());
+        });
+        OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config);
+        localizer.setPose(Pose.zero());
+        localizer.update();
+
+        Pose position = null;
+
+        waitForStart();
+
+        while (!isStopRequested()) {
+            localizer.update();
+            position = localizer.pose();
+            telemetry.addData("heading", localizer.pose().heading());
+            telemetry.update();
+        }
+
+        if (position == null) {
+            return null;
+        }
+
+        return List.of(-position.x() / 2.0, -position.y() / 2.0);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OctoQuadTuner.java

@@ -0,0 +1,350 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.math.Pose;
+import com.pedropathing.revhub.localizers.OctoQuadConfig;
+import com.pedropathing.revhub.localizers.OctoQuadLocalizer;
+import com.pedropathing.tuning.autotune.Inputs;
+import com.pedropathing.tuning.autotune.Procedure;
+import com.pedropathing.tuning.autotune.TuningOpMode;
+import com.pedropathing.utils.Angle;
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+import java.util.List;
+
+public class OctoQuadTuner extends Procedure {
+    enum PodType {
+        SWING_ARM,
+        FOUR_BAR,
+        CUSTOM
+    }
+
+    public static double SWING_ARM = 336.877962768;
+    public static double FOUR_BAR = 505.316944406;
+
+    public OctoQuadTuner() {
+        super("OctoQuad Tuner", "A procedure for tuning the OctoQuad localizer.");
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        Inputs inputs = inputs("Setup", "Set OctoQuad HardwareMap Name and Odometry Pod Type");
+        Inputs.Field<String> octoquadName = inputs.s("HardwareMap Name").withDefault("octoquad");
+        Inputs.Field<Integer> xPort =  inputs.i("Forward Pod Port").withDefault(0);
+        Inputs.Field<Integer> yPort =  inputs.i("Strafe Pod Port").withDefault(1);
+        Inputs.Field<OctoQuadTuner.PodType> podType = inputs.e("Odometry Pod Type", OctoQuadTuner.PodType.class).withDefault(OctoQuadTuner.PodType.FOUR_BAR);
+        Inputs.Field<OctoQuad.I2cRecoveryMode> recoveryMode = inputs.e("Recovery Mode", OctoQuad.I2cRecoveryMode.class).withDefault(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR);
+        awaitInputs(inputs);
+
+        double customPodScalar = 0;
+
+        Inputs inputsHeadingScalar = inputs("Custom Scalar Identification Turns", "Set the number of times you will turn your robot.");
+        Inputs.Field<Integer> turns = inputsHeadingScalar.i("Turns").withDefault(10);
+        awaitInputs(inputsHeadingScalar);
+        double headingScalar = runOpMode(new OctoQuadHeadingScalar(octoquadName.get(), turns.get(), xPort.get(), yPort.get()));
+
+        if (podType.get() == PodType.CUSTOM) {
+            Inputs inputsCustom = inputs("Custom Scalar Identification Push Distance", "Set the distance you will push your robot forward in inches");
+            Inputs.Field<Double> distance = inputsCustom.d("Distance").withDefault(48.0);
+            awaitInputs(inputsCustom);
+            customPodScalar = runOpMode(new OctoQuadCustomPodScalar(distance.get(), octoquadName.get(), xPort.get(), yPort.get()));
+        }
+
+        boolean forwardPodReversed = runOpMode(new OctoQuadForwardDirection(octoquadName.get(), podType.get(), customPodScalar, headingScalar, xPort.get(), yPort.get()));
+        boolean strafePodReversed = runOpMode(new OctoQuadStrafeDirection(octoquadName.get(), podType.get(), customPodScalar, headingScalar, xPort.get(), yPort.get()));
+
+        List<Double> offsets = runOpMode(new OctoQuadOffsets(octoquadName.get(), podType.get(), customPodScalar, forwardPodReversed, strafePodReversed, headingScalar, xPort.get(), yPort.get()));
+
+        result("name", octoquadName.get());
+        result("headingScalar", headingScalar);
+
+        if (podType.get() == PodType.CUSTOM) {
+            result("podType", "Custom");
+            result("ticksPerUnit", customPodScalar);
+        } else {
+            result("podType", podType.get() == PodType.SWING_ARM ? SWING_ARM : FOUR_BAR);
+        }
+
+        result("xPodDirection", forwardPodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+        result("yPodDirection", strafePodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+        result("xPodOffset", offsets.get(0));
+        result("yPodOffset", offsets.get(1));
+
+        code(Language.JAVA,"public static OctoQuadConfig localizerConfig = new OctoQuadConfig(c -> {\n" +
+                "    c.name.set(\"" + octoquadName.get() + "\");\n" +
+                "    c.xPodPort.set(" + xPort.get() + ");\n" +
+                "    c.yPodPort.set(" + yPort.get() + ");\n" +
+                (podType.get() == PodType.CUSTOM ? "    c.ticksPerUnit.set(" + customPodScalar + ");\n" : "    c.ticksPerUnit.set(" + (podType.get() == OctoQuadTuner.PodType.SWING_ARM ? SWING_ARM : FOUR_BAR) + ");\n") +
+                "    c.xPodOffset.set(" + offsets.get(0) + ");\n" +
+                "    c.yPodOffset.set(" + offsets.get(1) + ");\n" +
+                "    c.xPodDirection.set(" + (forwardPodReversed ? "OctoQuad.EncoderDirection.REVERSE" : "OctoQuad.EncoderDirection.FORWARD") + ");\n" +
+                "    c.yPodDirection.set(" + (strafePodReversed ? "OctoQuad.EncoderDirection.REVERSE" : "OctoQuad.EncoderDirection.FORWARD") + ");\n" +
+                "    c.globalDistanceUnit.set(DistanceUnit.INCH);\n" +
+                "    c.offsetUnits.set(DistanceUnit.INCH);\n" +
+                "    c.i2cRecoveryMode.set(OctoQuad.I2cRecoveryMode." + recoveryMode.get() + ");\n" +
+                "    c.headingScalar.set(" + headingScalar + ");\n" +
+                "});");
+    }
+}
+
+class OctoQuadHeadingScalar extends TuningOpMode<Double> {
+    String name;
+    int turns;
+    double totalHeading = 0;
+    double prevHeading = 0;
+    int xPodPort;
+    int yPodPort;
+
+    public OctoQuadHeadingScalar(String name, int turns, int xPodPort, int yPodPort) {
+        super("Heading Scalar Identification",
+                "Determines the scalar for the custom pods of the OctoQuad localizer. \n"
+                        + "Turn your robot " + turns * 360 + " degrees exactly ("+ turns + " times) exactly and then stop the OpMode.",
+                true);
+        this.name = name;
+        this.turns = turns;
+        this.xPodPort = xPodPort;
+        this.yPodPort = yPodPort;
+    }
+
+    @Override
+    protected Double runTuningOpMode() throws InterruptedException {
+        OctoQuadConfig config = new OctoQuadConfig(c -> {
+            c.name.set(name);
+            c.xPodPort.set(xPodPort);
+            c.yPodPort.set(yPodPort);
+            c.ticksPerUnit.set(1.0);
+            c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+        });
+
+        OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        localizer.update();
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+
+            if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y() || localizer.pose().heading() != Pose.zero().heading()) {
+                double currentHeading = localizer.pose().heading();
+                totalHeading += Angle.normalizeSigned(currentHeading - prevHeading);
+                prevHeading = currentHeading;
+            }
+        }
+        return Math.abs((turns * Math.PI * 2 / totalHeading));
+    }
+}
+
+class OctoQuadCustomPodScalar extends TuningOpMode<Double> {
+    String name;
+    double distance;
+    int xPodPort, yPodPort;
+
+    public OctoQuadCustomPodScalar(Double distance, String name, int xPodPort, int yPodPort) {
+        super("Custom Scalar Identification",
+                "Determines the scalar for the custom pods of the OctoQuad localizer. \n"
+                        + "Push your robot forward " + distance + " inches exactly and then stop the Opmode",
+                true);
+        this.name = name;
+        this.distance = distance;
+        this.xPodPort = xPodPort;
+        this.yPodPort = yPodPort;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        OctoQuadConfig config = new OctoQuadConfig(c -> {
+            c.name.set(name);
+            c.xPodPort.set(xPodPort);
+            c.yPodPort.set(yPodPort);
+            c.ticksPerUnit.set(1.0);
+            c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+        });
+        OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        waitForStart();
+
+        double startTicks = localizer.octoQuad.readAllEncoderData().positions[xPodPort];
+        double lastLastTicksPerInch = 0;
+        double lastTicksPerInch = 0;
+
+        while (!isStopRequested()) {
+            localizer.update();
+
+            double pos = localizer.octoQuad.readAllEncoderData().positions[xPodPort];
+            lastLastTicksPerInch = lastTicksPerInch;
+            lastTicksPerInch = Math.abs(pos - startTicks) / distance;
+        }
+
+        return lastLastTicksPerInch;
+    }
+}
+
+class OctoQuadForwardDirection extends TuningOpMode<Boolean> {
+    String name;
+    OctoQuadTuner.PodType podType;
+    double customPodScalar;
+    double headingScalar;
+    int xPodPort, yPodPort;
+
+    public OctoQuadForwardDirection(String name, OctoQuadTuner.PodType podType, double customPodScalar, double headingScalar,
+                                    int xPodPort, int yPodPort) {
+        super("Forward Direction Identification",
+                "Determines if your forward pod needs to be reversed. \n"
+                        + "Push your robot forward and then stop the Opmode",
+                true);
+        this.name = name;
+        this.podType = podType;
+        this.customPodScalar = customPodScalar;
+        this.headingScalar = headingScalar;
+        this.xPodPort = xPodPort;
+        this.yPodPort = yPodPort;
+    }
+
+    @Override
+    protected Boolean runTuningOpMode() {
+        OctoQuadConfig config = new OctoQuadConfig(c -> {
+            c.name.set(name);
+            c.xPodPort.set(xPodPort);
+            c.yPodPort.set(yPodPort);
+            c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+            c.headingScalar.set(headingScalar);
+            if (podType == OctoQuadTuner.PodType.CUSTOM) {
+                c.ticksPerUnit.set(customPodScalar);
+            } else {
+                c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR);
+            }
+        });
+        OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+        }
+
+        return localizer.pose().x() < 0;
+    }
+}
+
+class OctoQuadStrafeDirection extends TuningOpMode<Boolean> {
+    String name;
+    OctoQuadTuner.PodType podType;
+    double customPodScalar;
+    double headingScalar;
+    int xPodPort, yPodPort;
+
+    public OctoQuadStrafeDirection(String name, OctoQuadTuner.PodType podType, double customPodScalar, double headingScalar,
+                                   int xPodPort, int yPodPort) {
+        super("Strafe Direction Identification",
+                "Determines if your strafe pod needs to be reversed. \n"
+                        + "Push your robot to the left and then stop the Opmode",
+                true);
+        this.name = name;
+        this.podType = podType;
+        this.customPodScalar = customPodScalar;
+        this.headingScalar = headingScalar;
+        this.xPodPort = xPodPort;
+        this.yPodPort = yPodPort;
+    }
+
+    @Override
+    protected Boolean runTuningOpMode() {
+        OctoQuadConfig config = new OctoQuadConfig(c -> {
+            c.name.set(name);
+            c.xPodPort.set(xPodPort);
+            c.yPodPort.set(yPodPort);
+            c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+            c.headingScalar.set(headingScalar);
+
+            if (podType == OctoQuadTuner.PodType.CUSTOM) {
+                c.ticksPerUnit.set(customPodScalar);
+            } else {
+                c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR);
+            }
+        });
+        OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+        }
+
+        return localizer.pose().y() < 0;
+    }
+}
+
+class OctoQuadOffsets extends TuningOpMode<List<Double>> {
+    String name;
+    OctoQuadTuner.PodType podType;
+    double customPodScalar;
+    boolean forwardPodReversed, strafePodReversed;
+    double headingScalar;
+    Pose previous;
+    int xPodPort, yPodPort;
+
+    public OctoQuadOffsets(String name, OctoQuadTuner.PodType podType, double customPodScalar, Boolean forwardPodReversed, Boolean strafePodReversed, double headingScalar,
+                           int xPodPort, int yPodPort) {
+        super("OctoQuadOffsets Identification",
+                "Automatically identifies the offsets for your OctoQuad localizer. \n"
+                        + "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode",
+                true);
+        this.name = name;
+        this.podType = podType;
+        this.customPodScalar = customPodScalar;
+        this.forwardPodReversed = forwardPodReversed;
+        this.strafePodReversed = strafePodReversed;
+        this.headingScalar = headingScalar;
+        this.xPodPort = xPodPort;
+        this.yPodPort = yPodPort;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        OctoQuadConfig config = new OctoQuadConfig(c -> {
+            c.name.set(name);
+            c.xPodPort.set(xPodPort);
+            c.yPodPort.set(yPodPort);
+            c.xPodDirection.set(forwardPodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+            c.yPodDirection.set(strafePodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+            if (podType.equals(OctoQuadTuner.PodType.CUSTOM)) {
+                c.encoderResolutionUnit.set(DistanceUnit.INCH);
+                c.ticksPerUnit.set(customPodScalar);
+            } else {
+                c.encoderResolutionUnit.set(DistanceUnit.INCH);
+                c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR);
+            }
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+            c.headingScalar.set(headingScalar);
+        });
+        OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        localizer.update();
+
+        waitForStart();
+
+        while (!isStopRequested()) {
+            previous = localizer.pose();
+            localizer.update();
+            telemetry.addData("heading", localizer.pose().heading());
+            telemetry.update();
+        }
+
+        if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) {
+            previous =  localizer.pose();
+        }
+
+        return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0));
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/PinpointTuner.java

@@ -0,0 +1,256 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.math.Pose;
+import com.pedropathing.revhub.localizers.PinpointConfig;
+import com.pedropathing.revhub.localizers.PinpointLocalizer;
+import com.pedropathing.tuning.autotune.Inputs;
+import com.pedropathing.tuning.autotune.Procedure;
+import com.pedropathing.tuning.autotune.TuningOpMode;
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+import java.util.*;
+
+public class PinpointTuner extends Procedure {
+    enum PodType {
+        SWING_ARM,
+        FOUR_BAR,
+        CUSTOM
+    }
+    public PinpointTuner() {
+        super("Pinpoint Tuner", "A procedure for tuning the Pinpoint localizer.");
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        Inputs inputs = inputs("Setup", "Set Pinpoint HardwareMap Name and Odometry Pod Type");
+        Inputs.Field<String> pinpointName = inputs.s("HardwareMap Name").withDefault("pinpoint");
+        Inputs.Field<PodType> podType = inputs.e("Odometry Pod Type", PodType.class).withDefault(PodType.FOUR_BAR);
+        awaitInputs(inputs);
+
+        OptionalDouble customPodScalar = OptionalDouble.empty();
+
+        if (podType.get() == PodType.CUSTOM) {
+            Inputs inputsCustom = inputs("Custom Scalar Identification Push Distance", "Set the distance you will push your robot forward in inches");
+            Inputs.Field<Double> distance = inputsCustom.d("Distance").withDefault(48.0);
+            awaitInputs(inputsCustom);
+            customPodScalar = OptionalDouble.of(runOpMode(new PinpointCustomPodScalar(distance.get(), pinpointName.get())));
+        }
+
+        boolean forwardPodReversed = runOpMode(new PinpointForwardDirection(pinpointName.get(), podType.get(), customPodScalar));
+        boolean strafePodReversed = runOpMode(new PinpointStrafeDirection(pinpointName.get(), podType.get(), customPodScalar));
+
+        List<Double> offsets = runOpMode(new PinpointOffsets(pinpointName.get(), podType.get(), customPodScalar, forwardPodReversed, strafePodReversed));
+
+        result("name", pinpointName.get());
+
+        if (customPodScalar.isPresent()) {
+            result("podType", "Custom");
+            result("ticksPerUnit", customPodScalar.getAsDouble());
+        } else {
+            result("podType", podType.get() == PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
+        }
+
+        result("xPodDirection", forwardPodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
+        result("yPodDirection", strafePodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
+        result("xPodOffset", offsets.get(0));
+        result("yPodOffset", offsets.get(1));
+
+        code(Language.JAVA,"public static PinpointConfig localizerConfig = new PinpointConfig(c -> {\n" +
+                "    c.name.set(\"" + pinpointName.get() + "\");\n" +
+                (customPodScalar.isPresent() ? "    c.ticksPerUnit.set(OptionalDouble.of(" + customPodScalar.getAsDouble() + "));\n" : "    c.podType.set(" + (podType.get() == PodType.SWING_ARM ? "GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD" : "GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD") + ");\n") +
+                "    c.xPodOffset.set(" + offsets.get(0) + ");\n" +
+                "    c.yPodOffset.set(" + offsets.get(1) + ");\n" +
+                "    c.xPodDirection.set(" + (forwardPodReversed ? "GoBildaPinpointDriver.EncoderDirection.REVERSED" : "GoBildaPinpointDriver.EncoderDirection.FORWARD") + ");\n" +
+                "    c.yPodDirection.set(" + (strafePodReversed ? "GoBildaPinpointDriver.EncoderDirection.REVERSED" : "GoBildaPinpointDriver.EncoderDirection.FORWARD") + ");\n" +
+                "    c.globalDistanceUnit.set(DistanceUnit.INCH);\n" +
+                "    c.offsetUnits.set(DistanceUnit.INCH);\n" +
+                "});");
+    }
+}
+
+class PinpointCustomPodScalar extends TuningOpMode<Double> {
+
+    String name;
+    double distance;
+
+    public PinpointCustomPodScalar(Double distance, String name) {
+        super("Custom Scalar Identification",
+                "Determines the scalar for the custom pods of the Pinpoint localizer. \n"
+                        + "Push your robot forward " + distance + " inches exactly and then stop the Opmode",
+                true);
+        this.name = name;
+        this.distance = distance;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        PinpointConfig config = new PinpointConfig(c -> {
+            c.name.set(name);
+            c.ticksPerUnit.set(OptionalDouble.of(1.0));
+            c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+        });
+        PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+        }
+        return Math.abs((localizer.pose().x() / distance));
+    }
+}
+
+class PinpointForwardDirection extends TuningOpMode<Boolean> {
+    String name;
+    PinpointTuner.PodType podType;
+    OptionalDouble customPodScalar;
+
+    public PinpointForwardDirection(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar) {
+        super("Forward Direction Identification",
+                "Determines if your forward pod needs to be reversed. \n"
+                        + "Push your robot forward and then stop the Opmode",
+                true);
+        this.name = name;
+        this.podType = podType;
+        this.customPodScalar = customPodScalar;
+    }
+
+    @Override
+    protected Boolean runTuningOpMode() {
+        PinpointConfig config = new PinpointConfig(c -> {
+            c.name.set(name);
+            c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+            if (customPodScalar.isPresent()) {
+                c.encoderResolutionUnit.set(DistanceUnit.INCH);
+                c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble()));
+            } else {
+                c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
+            }
+        });
+        PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        waitForStart();
+
+        while (!isStopRequested()) {
+            localizer.update();
+        }
+
+        return localizer.pose().x() < 0;
+    }
+}
+
+class PinpointStrafeDirection extends TuningOpMode<Boolean> {
+    String name;
+    PinpointTuner.PodType podType;
+    OptionalDouble customPodScalar;
+
+    public PinpointStrafeDirection(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar) {
+        super("Strafe Direction Identification",
+                "Determines if your strafe pod needs to be reversed. \n"
+                        + "Push your robot to the left and then stop the Opmode",
+                true);
+        this.name = name;
+        this.podType = podType;
+        this.customPodScalar = customPodScalar;
+    }
+
+    @Override
+    protected Boolean runTuningOpMode() {
+        PinpointConfig config = new PinpointConfig(c -> {
+            c.name.set(name);
+            c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+            if (customPodScalar.isPresent()) {
+                c.encoderResolutionUnit.set(DistanceUnit.INCH);
+                c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble()));
+            } else {
+                c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
+            }
+        });
+        PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+        }
+
+        return localizer.pose().y() < 0;
+    }
+}
+
+class PinpointOffsets extends TuningOpMode<List<Double>> {
+    String name;
+    PinpointTuner.PodType podType;
+    OptionalDouble customPodScalar =  OptionalDouble.empty();
+    boolean forwardPodReversed, strafePodReversed;
+    private Pose previous = Pose.zero();
+
+    public PinpointOffsets(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar, Boolean forwardPodReversed, Boolean strafePodReversed) {
+        super("Offsets Identification",
+                "Automatically identifies the offsets for your Pinpoint localizer. \n"
+                        + "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode",
+                true);
+        this.name = name;
+        this.podType = podType;
+        if (customPodScalar.isPresent()) {
+            this.customPodScalar = customPodScalar;
+        }
+        this.forwardPodReversed = forwardPodReversed;
+        this.strafePodReversed = strafePodReversed;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        PinpointConfig config = new PinpointConfig(c -> {
+            c.name.set(name);
+            c.xPodDirection.set(forwardPodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            c.yPodDirection.set(strafePodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
+            if (customPodScalar.isPresent()) {
+                c.encoderResolutionUnit.set(DistanceUnit.INCH);
+                c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble()));
+                c.resetMode.set(PinpointLocalizer.ResetMode.RESET_AND_RECALIBRATE_IMU);
+            } else {
+                c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
+            }
+            c.xPodOffset.set(0.0);
+            c.yPodOffset.set(0.0);
+            c.globalDistanceUnit.set(DistanceUnit.INCH);
+            c.offsetUnits.set(DistanceUnit.INCH);
+        });
+        PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
+        if (customPodScalar.isPresent()) {
+            localizer.reset();
+        }
+        localizer.setPose(Pose.zero());
+        localizer.update();
+
+
+        waitForStart();
+
+        localizer.setPose(Pose.zero());
+
+        while (!isStopRequested()) {
+            previous = localizer.pose();
+            localizer.update();
+
+            telemetry.addData("heading", localizer.pose().heading());
+            telemetry.addData("pose", localizer.pose());
+            telemetry.addData("previous", previous);
+            telemetry.update();
+        }
+
+        if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) {
+            previous =  localizer.pose();
+        }
+
+        return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0));
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/Tests.java

@@ -0,0 +1,327 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.algorithm.Algorithm;
+import com.pedropathing.drivetrain.DrivePowers;
+import com.pedropathing.drivetrain.Drivetrain;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.localization.Localizer;
+import com.pedropathing.math.Pose;
+import com.pedropathing.paths.Path;
+import com.pedropathing.paths.interpolator.Interpolator;
+import com.pedropathing.tuning.autotune.DisplayName;
+import com.pedropathing.tuning.autotune.Inputs;
+import com.pedropathing.tuning.autotune.Procedure;
+import com.pedropathing.tuning.autotune.TuningOpMode;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import java.util.function.Function;
+import java.util.function.Supplier;
+
+import static com.pedropathing.api.Paths.curve;
+import static com.pedropathing.api.Paths.line;
+
+public class Tests extends Procedure {
+    enum Test {
+        @DisplayName("Hold Test")
+        HOLD,
+        @DisplayName("Line Test")
+        LINE,
+        @DisplayName("Curve Test")
+        CURVED,
+        @DisplayName("Interpolation Test")
+        INTERPOLATION_CURVED,
+        @DisplayName("Localization Test")
+        LOCALIZATION,
+        @DisplayName("Driving Test")
+        DRIVING,
+        @DisplayName("Pose Test")
+        POSE
+
+    }
+    Function<HardwareMap, Drivetrain> drivetrainFunction;
+    Function<HardwareMap, Localizer> localizerFunction;
+    Supplier<Algorithm> algorithmSupplier;
+    Function<HardwareMap, Follower> followerFunction;
+
+    public Tests(Function<HardwareMap, Drivetrain> drivetrainFunction, Function<HardwareMap, Localizer> localizerFunction, Supplier<Algorithm> algorithmSupplier) {
+        super("Tests", "A procedure for testing the Follower.");
+        this.drivetrainFunction = drivetrainFunction;
+        this.localizerFunction = localizerFunction;
+        this.algorithmSupplier = algorithmSupplier;
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        boolean completed = false;
+        boolean algorithm = true, localizer = true, drivetrain = true;
+
+        if (algorithmSupplier == null)
+            algorithm = false;
+
+        if (localizerFunction == null)
+            localizer = false;
+
+        if (drivetrainFunction == null)
+            drivetrain = false;
+
+        if (algorithm && localizer && drivetrain)
+            followerFunction = (hardwareMap) -> new Follower(localizerFunction.apply(hardwareMap), drivetrainFunction.apply(hardwareMap), algorithmSupplier.get());
+
+        Inputs inputs = inputs("Select", "Select");
+        Inputs.Field<Test> selectedTest = inputs.e("Test", Test.class).withDefault(Test.LINE);
+        Inputs.Field<Double> distance = inputs.d("Distance").withDefault(48.0);
+
+        awaitInputs(inputs);
+
+        switch (selectedTest.get()) {
+            case HOLD:
+                if (!algorithm)
+                    throw new IllegalArgumentException("Algorithm is required for Hold Test.");
+                completed = runOpMode(new TestsHold(followerFunction));
+                break;
+            case LINE:
+                if (!algorithm)
+                    throw new IllegalArgumentException("Algorithm is required for Hold Test.");
+                completed = runOpMode(new TestsLine(followerFunction, distance.get()));
+                break;
+            case CURVED:
+                if (!algorithm)
+                    throw new IllegalArgumentException("Algorithm is required for Hold Test.");
+                completed = runOpMode(new TestsCurve(followerFunction, distance.get()));
+                break;
+            case INTERPOLATION_CURVED:
+                if (!algorithm)
+                    throw new IllegalArgumentException("Algorithm is required for Hold Test.");
+                completed = runOpMode(new TestsInterpolation(followerFunction, distance.get()));
+                break;
+            case LOCALIZATION:
+                if (!drivetrain)
+                    throw new IllegalArgumentException("Drivetrain is required for Localization Test.");
+                if (!localizer)
+                    throw new IllegalArgumentException("Localizer is required for Localization Test.");
+                completed = runOpMode(new TestsLocalization(drivetrainFunction, localizerFunction));
+                break;
+            case POSE:
+                if (!localizer)
+                    throw new IllegalArgumentException("Localizer is required for Pose Test.");
+                completed = runOpMode(new TestsPose(localizerFunction));
+                break;
+            case DRIVING:
+                if (!drivetrain)
+                    throw new IllegalArgumentException("Drivetrain is required for Driving Test.");
+                completed = runOpMode(new TestsDriving(drivetrainFunction));
+                break;
+        }
+
+        result("Completed", completed);
+    }
+}
+
+class TestsHold extends TuningOpMode<Boolean> {
+    Function<HardwareMap, Follower> followerFunction;
+
+    public TestsHold(Function<HardwareMap, Follower> followerFunction) {
+        super("Hold Test", "Tests the Follower's ability to hold a position.", true);
+        this.followerFunction = followerFunction;
+    }
+
+    @Override
+    public Boolean runTuningOpMode() throws InterruptedException {
+        Follower follower = followerFunction.apply(hardwareMap);
+        follower.setPose(Pose.zero());
+        waitForStart();
+        follower.hold(Pose.zero());
+        while (opModeIsActive()) {
+            follower.update();
+        }
+        return true;
+    }
+}
+
+class TestsLine extends TuningOpMode<Boolean> {
+    Function<HardwareMap, Follower> followerFunction;
+    double distance;
+
+    public TestsLine(Function<HardwareMap, Follower> followerFunction, double distance) {
+        super("Line Test", "Tests the Follower's ability to follow a line.", true);
+        this.followerFunction = followerFunction;
+        this.distance = distance;
+    }
+
+    @Override
+    public Boolean runTuningOpMode() throws InterruptedException {
+        Follower follower = followerFunction.apply(hardwareMap);
+        follower.setPose(Pose.zero());
+
+        double distance = 48;
+        boolean forward = true;
+
+        Path path1 = line(Pose.zero(), new Pose(distance,0, 0)).constant(0);
+        Path path2 = line(new Pose(distance,0, 0), Pose.zero()).constant(0);
+
+        waitForStart();
+        follower.follow(path1);
+
+        while (opModeIsActive()) {
+            follower.update();
+            if (follower.atParametricEnd()) {
+                if (forward) {
+                    follower.follow(path2);
+                } else {
+                    follower.follow(path1);
+                }
+                forward = !forward;
+            }
+        }
+        return true;
+    }
+}
+
+class TestsCurve extends TuningOpMode<Boolean> {
+    Function<HardwareMap, Follower> followerFunction;
+    double distance;
+
+    public TestsCurve(Function<HardwareMap, Follower> followerFunction, double distance) {
+        super("Curve Test", "Tests the Follower's ability to follow a curve.", true);
+        this.followerFunction = followerFunction;
+        this.distance = distance;
+    }
+
+    @Override
+    public Boolean runTuningOpMode() throws InterruptedException {
+        Follower follower = followerFunction.apply(hardwareMap);
+        follower.setPose(Pose.zero());
+
+        double distance = 48;
+        boolean forward = true;
+
+        Path path1 = curve(Pose.zero(), new Pose(distance + 0,0), new Pose(distance,distance)).tangent();
+        Path path2 = curve(new Pose(distance,distance), new Pose(distance,0), Pose.zero()).tangent();
+
+        waitForStart();
+        follower.follow(path1);
+
+        while (opModeIsActive()) {
+            follower.update();
+            if (follower.atParametricEnd()) {
+                if (forward) {
+                    follower.follow(path2);
+                } else {
+                    follower.follow(path1);
+                }
+                forward = !forward;
+            }
+        }
+        return true;
+    }
+}
+
+class TestsInterpolation extends TuningOpMode<Boolean> {
+    Function<HardwareMap, Follower> followerFunction;
+    double distance;
+
+    public TestsInterpolation(Function<HardwareMap, Follower> followerFunction, double distance) {
+        super("Interpolation Curve Test", "Tests the Follower's ability to follow a curve with several interpolations.", true);
+        this.followerFunction = followerFunction;
+        this.distance = distance;
+    }
+
+    @Override
+    public Boolean runTuningOpMode() throws InterruptedException {
+        Follower follower = followerFunction.apply(hardwareMap);
+        follower.setPose(Pose.zero());
+
+        double distance = 48;
+        boolean forward = true;
+
+        Path path1 = curve(Pose.zero(), new Pose(distance + 0,0), new Pose(distance,distance)).heading((curve, t) -> Math.PI);
+        Path path2 = curve(new Pose(distance,distance), new Pose(distance,0), Pose.zero()).heading(Interpolator.piecewise().until(0.5, Interpolator.tangent).until(1.0, Interpolator.constant(0)));
+
+        waitForStart();
+        follower.follow(path1);
+
+        while (opModeIsActive()) {
+            follower.update();
+            if (follower.atParametricEnd()) {
+                if (forward) {
+                    follower.follow(path2);
+                } else {
+                    follower.follow(path1);
+                }
+                forward = !forward;
+            }
+        }
+        return true;
+    }
+}
+
+class TestsLocalization extends TuningOpMode<Boolean> {
+    Function<HardwareMap, Drivetrain> drivetrainFunction;
+    Function<HardwareMap, Localizer> localizerFunction;
+
+    public TestsLocalization(Function<HardwareMap, Drivetrain> drivetrainFunction, Function<HardwareMap, Localizer> localizerFunction) {
+        super("Localization Test", "Verifies localization and manual control.", true);
+        this.drivetrainFunction = drivetrainFunction;
+        this.localizerFunction = localizerFunction;
+    }
+
+    @Override
+    public Boolean runTuningOpMode() throws InterruptedException {
+        Localizer localizer = localizerFunction.apply(hardwareMap);
+        Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap);
+
+        localizer.setPose(Pose.zero());
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            drivetrain.drive(new DrivePowers(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x), false);
+            localizer.update();
+            telemetry.addData("Pose", localizer.pose());
+            telemetry.update();
+        }
+        return true;
+    }
+}
+
+class TestsDriving extends TuningOpMode<Boolean> {
+    Function<HardwareMap, Drivetrain> drivetrainFunction;
+
+    public TestsDriving(Function<HardwareMap, Drivetrain> drivetrainFunction) {
+        super("Driving Test", "Tests raw drivetrain control without localization.", true);
+        this.drivetrainFunction = drivetrainFunction;
+    }
+
+    @Override
+    public Boolean runTuningOpMode() throws InterruptedException {
+        Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap);
+        waitForStart();
+        while (opModeIsActive()) {
+            drivetrain.drive(new DrivePowers(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x), true);
+        }
+        return true;
+    }
+}
+
+class TestsPose extends TuningOpMode<Boolean> {
+    Function<HardwareMap, Localizer> localizerFunction;
+
+    public TestsPose(Function<HardwareMap, Localizer> localizerFunction) {
+        super("Pose Test", "Verifies localizer output without a drivetrain.", true);
+        this.localizerFunction = localizerFunction;
+    }
+
+    @Override
+    public Boolean runTuningOpMode() throws InterruptedException {
+        Localizer localizer = localizerFunction.apply(hardwareMap);
+        waitForStart();
+        while (opModeIsActive()) {
+            localizer.update();
+            telemetry.addData("Pose", localizer.pose());
+            telemetry.update();
+        }
+        return true;
+    }
+}
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelIMUTuner.java

@@ -0,0 +1,328 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.math.Pose;
+import com.pedropathing.revhub.localizers.Encoder;
+import com.pedropathing.revhub.localizers.RevHubIMU;
+import com.pedropathing.revhub.localizers.ThreeWheelIMUConfig;
+import com.pedropathing.revhub.localizers.ThreeWheelIMULocalizer;
+import com.pedropathing.tuning.autotune.Inputs;
+import com.pedropathing.tuning.autotune.Procedure;
+import com.pedropathing.tuning.autotune.TuningOpMode;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import java.util.List;
+
+public class ThreeWheelIMUTuner extends Procedure {
+
+    private static String leftEncoderName = "lf";
+    private static String rightEncoderName = "rr";
+    private static String strafeEncoderName = "lr";
+    private static String imuName = "imu";
+    private static RevHubOrientationOnRobot.LogoFacingDirection logoDirection =
+            RevHubOrientationOnRobot.LogoFacingDirection.UP;
+    private static RevHubOrientationOnRobot.UsbFacingDirection usbDirection =
+            RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
+
+    public ThreeWheelIMUTuner() {
+        super("Three Wheel + IMU Tuner", "Tune three odometry pods with an IMU");
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        Inputs setup = inputs("Encoder + IMU Setup",
+                "Set encoder motor ports, IMU HardwareMap name, and Control Hub orientation.");
+        Inputs.Field<String> leftEncoder = setup.s("Left Encoder Motor Name").withDefault("lf");
+        Inputs.Field<String> rightEncoder = setup.s("Right Encoder Motor Name").withDefault("rr");
+        Inputs.Field<String> strafeEncoder = setup.s("Strafe Encoder Motor Name").withDefault("lr");
+        Inputs.Field<String> imu = setup.s("IMU HardwareMap Name").withDefault("imu");
+        Inputs.Field<RevHubOrientationOnRobot.LogoFacingDirection> logo =
+                setup.e("Logo Facing Direction", RevHubOrientationOnRobot.LogoFacingDirection.class)
+                        .withDefault(RevHubOrientationOnRobot.LogoFacingDirection.UP);
+        Inputs.Field<RevHubOrientationOnRobot.UsbFacingDirection> usb =
+                setup.e("USB Facing Direction", RevHubOrientationOnRobot.UsbFacingDirection.class)
+                        .withDefault(RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD);
+        awaitInputs(setup);
+        leftEncoderName = leftEncoder.get();
+        rightEncoderName = rightEncoder.get();
+        strafeEncoderName = strafeEncoder.get();
+        imuName = imu.get();
+        logoDirection = logo.get();
+        usbDirection = usb.get();
+
+        Inputs resolution = inputs("Encoder Resolution Identification",
+                "Set a positive push distance in inches. Keep the robot straight during each push.");
+        Inputs.Field<Double> distance = resolution.d("Distance").withDefault(48.0);
+        awaitInputs(resolution);
+        if (!(distance.get() > 0.0)) {
+            abort("Enter a positive distance in inches.");
+            return;
+        }
+
+        List<Double> left = measure("Left", distance.get());
+        if (left == null) {
+            return;
+        }
+        List<Double> right = measure("Right", distance.get());
+        if (right == null) {
+            return;
+        }
+        List<Double> strafe = measure("Strafe", distance.get());
+        if (strafe == null) {
+            return;
+        }
+
+        double leftTicksPerInch = left.get(0);
+        double rightTicksPerInch = right.get(0);
+        double strafeTicksPerInch = strafe.get(0);
+        double forwardTicksPerInch = 2.0 / (1.0 / leftTicksPerInch + 1.0 / rightTicksPerInch);
+
+        double forward = 1.0 / forwardTicksPerInch;
+        double lateral = 1.0 / strafeTicksPerInch;
+
+        List<Double> leftOffsets = runOpMode(new ThreeWheelIMUOffsets(
+                true, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
+        if (leftOffsets == null) {
+            abort("Left stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
+            return;
+        }
+        List<Double> rightOffsets = runOpMode(new ThreeWheelIMUOffsets(
+                false, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
+
+        if (rightOffsets == null) {
+            abort("Right stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
+            return;
+        }
+        ThreeWheelIMUConfig config = config(true, forward, lateral,
+                left.get(1), right.get(1), strafe.get(1));
+        config.leftPodY.set(leftOffsets.get(0));
+        config.rightPodY.set(rightOffsets.get(0));
+        config.turnTicksToRadians.set(forward);
+
+        Double turn = runOpMode(new ThreeWheelIMUTurn(config));
+        if (turn == null) {
+            abort("Turn stage ended without positive rotation. Rotate 360 degrees CCW, then press Stop.");
+            return;
+        }
+        double strafeX = (leftOffsets.get(1) + rightOffsets.get(1)) / 2.0 * turn / lateral;
+
+        result("leftEncoderName", leftEncoderName);
+        result("rightEncoderName", rightEncoderName);
+        result("strafeEncoderName", strafeEncoderName);
+        result("imuName", imuName);
+        result("logoDirection", logoDirection);
+        result("usbDirection", usbDirection);
+        result("leftPodY", leftOffsets.get(0));
+        result("rightPodY", rightOffsets.get(0));
+        result("strafePodX", strafeX);
+        result("leftTicksPerInch", leftTicksPerInch);
+        result("rightTicksPerInch", rightTicksPerInch);
+        result("forwardTicksPerInch", forwardTicksPerInch);
+        result("strafeTicksPerInch", strafeTicksPerInch);
+        result("forwardTicksToInches", forward);
+        result("strafeTicksToInches", lateral);
+        result("turnTicksToRadians", turn);
+        result("leftEncoderDirection", direction(left.get(1)));
+        result("rightEncoderDirection", direction(right.get(1)));
+        result("strafeEncoderDirection", direction(strafe.get(1)));
+
+        code(Language.JAVA,
+                "public static ThreeWheelIMUConfig localizerConfig = new ThreeWheelIMUConfig(c -> {\n" +
+                        "    c.leftEncoderName.set(\"" + leftEncoderName + "\");\n" +
+                        "    c.rightEncoderName.set(\"" + rightEncoderName + "\");\n" +
+                        "    c.strafeEncoderName.set(\"" + strafeEncoderName + "\");\n" +
+                        "    c.imuName.set(\"" + imuName + "\");\n" +
+                        "    c.imuOrientation.set(new RevHubOrientationOnRobot(\n" +
+                        "            RevHubOrientationOnRobot.LogoFacingDirection." + logoDirection.name() + ",\n" +
+                        "            RevHubOrientationOnRobot.UsbFacingDirection." + usbDirection.name() + "\n" +
+                        "    ));\n" +
+                        "    c.leftPodY.set(" + leftOffsets.get(0) + ");\n" +
+                        "    c.rightPodY.set(" + rightOffsets.get(0) + ");\n" +
+                        "    c.strafePodX.set(" + strafeX + ");\n" +
+                        "    c.forwardTicksToInches.set(" + forward + ");\n" +
+                        "    c.strafeTicksToInches.set(" + lateral + ");\n" +
+                        "    c.turnTicksToRadians.set(" + turn + ");\n" +
+                        "    c.leftEncoderDirection.set(" + direction(left.get(1)) + ");\n" +
+                        "    c.rightEncoderDirection.set(" + direction(right.get(1)) + ");\n" +
+                        "    c.strafeEncoderDirection.set(" + direction(strafe.get(1)) + ");\n" +
+                        "});");
+    }
+
+    private List<Double> measure(String pod, double distance) throws InterruptedException {
+        List<Double> measured = runOpMode(new ThreeWheelIMUResolution(pod, distance));
+        if (measured == null) {
+            abort(pod + " stage ended without a nonzero measurement. Check the displayed ticks, complete the push, then press Stop.");
+            return null;
+        }
+        return measured;
+    }
+
+    static String direction(double direction) {
+        return direction == Encoder.REVERSE ? "Encoder.REVERSE" : "Encoder.FORWARD";
+    }
+
+    static ThreeWheelIMUConfig config(boolean left, double forward, double strafe,
+                                      double leftDirection, double rightDirection, double strafeDirection) {
+        return new ThreeWheelIMUConfig(c -> {
+            c.leftEncoderName.set(leftEncoderName);
+            c.rightEncoderName.set(rightEncoderName);
+            c.strafeEncoderName.set(strafeEncoderName);
+            c.imuName.set(imuName);
+            c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(logoDirection, usbDirection)));            c.leftPodY.set(left ? 0.0 : 1.0);
+            c.rightPodY.set(left ? -1.0 : 0.0);
+            c.strafePodX.set(0.0);
+            c.forwardTicksToInches.set(forward);
+            c.strafeTicksToInches.set(strafe);
+            c.turnTicksToRadians.set(0.0);
+            c.leftEncoderDirection.set(leftDirection);
+            c.rightEncoderDirection.set(rightDirection);
+            c.strafeEncoderDirection.set(strafeDirection);
+        });
+    }
+
+    static ThreeWheelIMULocalizer localizer(HardwareMap map, ThreeWheelIMUConfig config) {
+        for (LynxModule hub : map.getAll(LynxModule.class)) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+        for (String name : new String[]{"lf", "lr", "rf", "rr"}) {
+            DcMotorEx motor = map.get(DcMotorEx.class, name);
+            motor.setPower(0);
+            motor.setDirection(name.equals("lf") || name.equals("lr")
+                    ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+            motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+        return new ThreeWheelIMULocalizer(map, config);
+    }
+}
+
+class ThreeWheelIMUResolution extends TuningOpMode<List<Double>> {
+
+    String pod;
+    double distance;
+
+    ThreeWheelIMUResolution(String pod, double distance) {
+        super(pod + " Encoder Resolution and Direction",
+                "After Start, push the robot " + (pod.equals("Strafe") ? "left " : "forward ") +
+                        distance + " inches exactly without turning. Stop moving, press Stop to save this measurement.", true);
+        this.pod = pod;
+        this.distance = distance;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        ThreeWheelIMUConfig config = ThreeWheelIMUTuner.config(!pod.equals("Right"), 1.0, 1.0,
+                Encoder.FORWARD, Encoder.FORWARD, Encoder.FORWARD);
+        ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        Pose position = null;
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+            position = localizer.pose();
+        }
+
+        if (position == null) {
+            return null;
+        }
+        double movement = pod.equals("Strafe") ? position.y() : position.x();
+        if (movement == 0.0) {
+            return null;
+        }
+        return List.of(Math.abs(movement / distance), movement < 0 ? Encoder.REVERSE : Encoder.FORWARD);
+    }
+}
+
+class ThreeWheelIMUOffsets extends TuningOpMode<List<Double>> {
+
+    boolean left;
+    double forward;
+    double strafe;
+    double leftDirection;
+    double rightDirection;
+    double strafeDirection;
+
+    ThreeWheelIMUOffsets(boolean left, double forward, double strafe,
+                         double leftDirection, double rightDirection, double strafeDirection) {
+        super((left ? "Left" : "Right") + " Pod Offset Identification",
+                "After Start, rotate exactly 180 degrees counterclockwise about the robot center. " +
+                        "Keep that center fixed. Stop moving, press Stop to save this measurement.", true);
+        this.left = left;
+        this.forward = forward;
+        this.strafe = strafe;
+        this.leftDirection = leftDirection;
+        this.rightDirection = rightDirection;
+        this.strafeDirection = strafeDirection;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        ThreeWheelIMUConfig config = ThreeWheelIMUTuner.config(left, forward, strafe,
+                leftDirection, rightDirection, strafeDirection);
+        boolean previousUseIMU = ThreeWheelIMULocalizer.useIMU;
+        ThreeWheelIMULocalizer.useIMU = false;
+        try {
+            ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config);
+            localizer.setPose(new Pose(0, 0));
+            localizer.update();
+            Pose position = null;
+
+            waitForStart();
+            while (!isStopRequested()) {
+                localizer.update();
+                position = localizer.pose();
+            }
+
+            if (position == null) {
+                return null;
+            }
+            return List.of(-position.x() / Math.PI, position.y() / Math.PI);
+        } finally {
+            ThreeWheelIMULocalizer.useIMU = previousUseIMU;
+        }
+    }
+}
+
+class ThreeWheelIMUTurn extends TuningOpMode<Double> {
+
+    ThreeWheelIMUConfig config;
+
+    ThreeWheelIMUTurn(ThreeWheelIMUConfig config) {
+        super("Turn Multiplier Identification",
+                "After Start, rotate exactly 360 degrees counterclockwise. " +
+                        "Stop moving, press Stop to save this measurement.", true);
+        this.config = config;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        boolean previousUseIMU = ThreeWheelIMULocalizer.useIMU;
+        ThreeWheelIMULocalizer.useIMU = false;
+        try {
+            ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config);
+            localizer.setPose(new Pose(0, 0));
+            localizer.update();
+            double startHeading = localizer.getTotalHeading();
+            Double heading = null;
+
+            waitForStart();
+            while (!isStopRequested()) {
+                localizer.update();
+                heading = localizer.getTotalHeading();
+            }
+
+            if (heading == null || heading <= startHeading) {
+                return null;
+            }
+            return config.turnTicksToRadians.get() * (2.0 * Math.PI) / (heading - startHeading);
+        } finally {
+            ThreeWheelIMULocalizer.useIMU = previousUseIMU;
+        }
+    }
+}
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelTuner.java

@@ -0,0 +1,290 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.math.Pose;
+import com.pedropathing.revhub.localizers.Encoder;
+import com.pedropathing.revhub.localizers.ThreeWheelConfig;
+import com.pedropathing.revhub.localizers.ThreeWheelLocalizer;
+import com.pedropathing.tuning.autotune.Inputs;
+import com.pedropathing.tuning.autotune.Procedure;
+import com.pedropathing.tuning.autotune.TuningOpMode;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import java.util.List;
+
+public class ThreeWheelTuner extends Procedure {
+
+    private static String leftEncoderName = "lf";
+    private static String rightEncoderName = "rr";
+    private static String strafeEncoderName = "lr";
+
+    public ThreeWheelTuner() {
+        super("Three Wheel Tuner", "Tune three odometry pods");
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        Inputs setup = inputs("Encoder Setup",
+                "Set the motor ports that the three odometry encoders are plugged into.");
+        Inputs.Field<String> leftEncoder = setup.s("Left Encoder Motor Name").withDefault("lf");
+        Inputs.Field<String> rightEncoder = setup.s("Right Encoder Motor Name").withDefault("rr");
+        Inputs.Field<String> strafeEncoder = setup.s("Strafe Encoder Motor Name").withDefault("lr");
+        awaitInputs(setup);
+        leftEncoderName = leftEncoder.get();
+        rightEncoderName = rightEncoder.get();
+        strafeEncoderName = strafeEncoder.get();
+
+        Inputs resolution = inputs("Encoder Resolution Identification",
+                "Set a positive push distance in inches. Keep the robot straight during each push.");
+        Inputs.Field<Double> distance = resolution.d("Distance").withDefault(48.0);
+        awaitInputs(resolution);
+        if (!(distance.get() > 0.0)) {
+            abort("Enter a positive distance in inches.");
+            return;
+        }
+
+        List<Double> left = measure("Left", distance.get());
+        if (left == null) {
+            return;
+        }
+        List<Double> right = measure("Right", distance.get());
+        if (right == null) {
+            return;
+        }
+        List<Double> strafe = measure("Strafe", distance.get());
+        if (strafe == null) {
+            return;
+        }
+
+        double leftTicksPerInch = left.get(0);
+        double rightTicksPerInch = right.get(0);
+        double strafeTicksPerInch = strafe.get(0);
+        double forwardTicksPerInch = 2.0 / (1.0 / leftTicksPerInch + 1.0 / rightTicksPerInch);
+
+        double forward = 1.0 / forwardTicksPerInch;
+        double lateral = 1.0 / strafeTicksPerInch;
+
+        List<Double> leftOffsets = runOpMode(new ThreeWheelOffsets(
+                true, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
+        if (leftOffsets == null) {
+            abort("Left stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
+            return;
+        }
+        List<Double> rightOffsets = runOpMode(new ThreeWheelOffsets(
+                false, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
+
+        if (rightOffsets == null) {
+            abort("Right stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
+            return;
+        }
+        ThreeWheelConfig config = config(true, forward, lateral,
+                left.get(1), right.get(1), strafe.get(1));
+        config.leftPodY.set(leftOffsets.get(0));
+        config.rightPodY.set(rightOffsets.get(0));
+        config.turnTicksToRadians.set(forward);
+
+        Double turn = runOpMode(new ThreeWheelTurn(config));
+        if (turn == null) {
+            abort("Turn stage ended without positive rotation. Rotate 360 degrees CCW, then press Stop.");
+            return;
+        }
+        double strafeX = (leftOffsets.get(1) + rightOffsets.get(1)) / 2.0 * turn / lateral;
+
+        result("leftEncoderName", leftEncoderName);
+        result("rightEncoderName", rightEncoderName);
+        result("strafeEncoderName", strafeEncoderName);
+        result("leftPodY", leftOffsets.get(0));
+        result("rightPodY", rightOffsets.get(0));
+        result("strafePodX", strafeX);
+        result("leftTicksPerInch", leftTicksPerInch);
+        result("rightTicksPerInch", rightTicksPerInch);
+        result("forwardTicksPerInch", forwardTicksPerInch);
+        result("strafeTicksPerInch", strafeTicksPerInch);
+        result("forwardTicksToInches", forward);
+        result("strafeTicksToInches", lateral);
+        result("turnTicksToRadians", turn);
+        result("leftEncoderDirection", direction(left.get(1)));
+        result("rightEncoderDirection", direction(right.get(1)));
+        result("strafeEncoderDirection", direction(strafe.get(1)));
+
+        code(Language.JAVA,
+                "public static ThreeWheelConfig localizerConfig = new ThreeWheelConfig(c -> {\n" +
+                        "    c.leftEncoderName.set(\"" + leftEncoderName + "\");\n" +
+                        "    c.rightEncoderName.set(\"" + rightEncoderName + "\");\n" +
+                        "    c.strafeEncoderName.set(\"" + strafeEncoderName + "\");\n" +
+                        "    c.leftPodY.set(" + leftOffsets.get(0) + ");\n" +
+                        "    c.rightPodY.set(" + rightOffsets.get(0) + ");\n" +
+                        "    c.strafePodX.set(" + strafeX + ");\n" +
+                        "    c.forwardTicksToInches.set(" + forward + ");\n" +
+                        "    c.strafeTicksToInches.set(" + lateral + ");\n" +
+                        "    c.turnTicksToRadians.set(" + turn + ");\n" +
+                        "    c.leftEncoderDirection.set(" + direction(left.get(1)) + ");\n" +
+                        "    c.rightEncoderDirection.set(" + direction(right.get(1)) + ");\n" +
+                        "    c.strafeEncoderDirection.set(" + direction(strafe.get(1)) + ");\n" +
+                        "});");
+    }
+
+    private List<Double> measure(String pod, double distance) throws InterruptedException {
+        List<Double> measured = runOpMode(new ThreeWheelResolution(pod, distance));
+        if (measured == null) {
+            abort(pod + " stage ended without a nonzero measurement. Check the displayed ticks, complete the push, then press Stop.");
+            return null;
+        }
+        return measured;
+    }
+
+    static String direction(double direction) {
+        return direction == Encoder.REVERSE ? "Encoder.REVERSE" : "Encoder.FORWARD";
+    }
+
+    static ThreeWheelConfig config(boolean left, double forward, double strafe,
+                                   double leftDirection, double rightDirection, double strafeDirection) {
+        return new ThreeWheelConfig(c -> {
+            c.leftEncoderName.set(leftEncoderName);
+            c.rightEncoderName.set(rightEncoderName);
+            c.strafeEncoderName.set(strafeEncoderName);
+            c.leftPodY.set(left ? 0.0 : 1.0);
+            c.rightPodY.set(left ? -1.0 : 0.0);
+            c.strafePodX.set(0.0);
+            c.forwardTicksToInches.set(forward);
+            c.strafeTicksToInches.set(strafe);
+            c.turnTicksToRadians.set(0.0);
+            c.leftEncoderDirection.set(leftDirection);
+            c.rightEncoderDirection.set(rightDirection);
+            c.strafeEncoderDirection.set(strafeDirection);
+        });
+    }
+
+    static ThreeWheelLocalizer localizer(HardwareMap map, ThreeWheelConfig config) {
+        for (LynxModule hub : map.getAll(LynxModule.class)) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+        for (String name : new String[]{"lf", "lr", "rf", "rr"}) {
+            DcMotorEx motor = map.get(DcMotorEx.class, name);
+            motor.setPower(0);
+            motor.setDirection(name.equals("lf") || name.equals("lr")
+                    ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+            motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+        return new ThreeWheelLocalizer(map, config);
+    }
+}
+
+class ThreeWheelResolution extends TuningOpMode<List<Double>> {
+
+    String pod;
+    double distance;
+
+    ThreeWheelResolution(String pod, double distance) {
+        super(pod + " Encoder Resolution and Direction",
+                "After Start, push the robot " + (pod.equals("Strafe") ? "left " : "forward ") +
+                        distance + " inches exactly without turning. Stop moving, press Stop to save this measurement.", true);
+        this.pod = pod;
+        this.distance = distance;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        ThreeWheelConfig config = ThreeWheelTuner.config(!pod.equals("Right"), 1.0, 1.0,
+                Encoder.FORWARD, Encoder.FORWARD, Encoder.FORWARD);
+        ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        Pose position = null;
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+            position = localizer.pose();
+        }
+
+        if (position == null) {
+            return null;
+        }
+        double movement = pod.equals("Strafe") ? position.y() : position.x();
+        if (movement == 0.0) {
+            return null;
+        }
+        return List.of(Math.abs(movement / distance), movement < 0 ? Encoder.REVERSE : Encoder.FORWARD);
+    }
+}
+
+class ThreeWheelOffsets extends TuningOpMode<List<Double>> {
+
+    boolean left;
+    double forward;
+    double strafe;
+    double leftDirection;
+    double rightDirection;
+    double strafeDirection;
+
+    ThreeWheelOffsets(boolean left, double forward, double strafe,
+                      double leftDirection, double rightDirection, double strafeDirection) {
+        super((left ? "Left" : "Right") + " Pod Offset Identification",
+                "After Start, rotate exactly 180 degrees counterclockwise about the robot center. " +
+                        "Keep that center fixed. Stop moving, press Stop to save this measurement.", true);
+        this.left = left;
+        this.forward = forward;
+        this.strafe = strafe;
+        this.leftDirection = leftDirection;
+        this.rightDirection = rightDirection;
+        this.strafeDirection = strafeDirection;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        ThreeWheelConfig config = ThreeWheelTuner.config(left, forward, strafe,
+                leftDirection, rightDirection, strafeDirection);
+        ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        localizer.update();
+        Pose position = null;
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+            position = localizer.pose();
+        }
+
+        if (position == null) {
+            return null;
+        }
+        return List.of(-position.x() / Math.PI, position.y() / Math.PI);
+    }
+}
+
+class ThreeWheelTurn extends TuningOpMode<Double> {
+
+    ThreeWheelConfig config;
+
+    ThreeWheelTurn(ThreeWheelConfig config) {
+        super("Turn Multiplier Identification",
+                "After Start, rotate exactly 360 degrees counterclockwise. " +
+                        "Stop moving, press Stop to save this measurement.", true);
+        this.config = config;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        localizer.update();
+        double startHeading = localizer.getTotalHeading();
+        Double heading = null;
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+            heading = localizer.getTotalHeading();
+        }
+
+        if (heading == null || heading <= startHeading) {
+            return null;
+        }
+        return config.turnTicksToRadians.get() * (2.0 * Math.PI) / (heading - startHeading);
+    }
+}
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/TwoWheelTuner.java

@@ -0,0 +1,404 @@
+package org.firstinspires.ftc.teamcode.pedro.procedures;
+
+import com.pedropathing.math.Pose;
+import com.pedropathing.revhub.localizers.Encoder;
+import com.pedropathing.revhub.localizers.RevHubIMU;
+import com.pedropathing.revhub.localizers.TwoWheelConfig;
+import com.pedropathing.revhub.localizers.TwoWheelLocalizer;
+import com.pedropathing.tuning.autotune.Inputs;
+import com.pedropathing.tuning.autotune.Procedure;
+import com.pedropathing.tuning.autotune.TuningOpMode;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+
+import java.util.List;
+
+public class TwoWheelTuner extends Procedure {
+
+    public TwoWheelTuner() {
+        super("Two Wheel Tuner", "A procedure for tuning the Two Wheel localizer.");
+    }
+
+    @Override
+    public void run() throws InterruptedException {
+        Inputs setup = inputs("Setup", "Set encoder, IMU, and Control Hub orientation");
+        Inputs.Field<String> forwardPodName = setup.s("Forward Encoder Motor Name").withDefault("lf");
+        Inputs.Field<String> strafePodName = setup.s("Strafe Encoder Motor Name").withDefault("rr");
+        Inputs.Field<String> imuName = setup.s("IMU HardwareMap Name").withDefault("imu");
+        Inputs.Field<RevHubOrientationOnRobot.LogoFacingDirection> logoDirection =
+                setup.e("Logo Facing Direction", RevHubOrientationOnRobot.LogoFacingDirection.class)
+                        .withDefault(RevHubOrientationOnRobot.LogoFacingDirection.UP);
+        Inputs.Field<RevHubOrientationOnRobot.UsbFacingDirection> usbDirection =
+                setup.e("USB Facing Direction", RevHubOrientationOnRobot.UsbFacingDirection.class)
+                        .withDefault(RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD);
+        awaitInputs(setup);
+
+        Inputs resolution = inputs("Encoder Resolution Identification", "Set the exact distance you will push the robot in inches");
+        Inputs.Field<Double> distance = resolution.d("Distance").withDefault(48.0);
+        awaitInputs(resolution);
+
+        TwoWheelSetup values = new TwoWheelSetup(
+                forwardPodName.get(),
+                strafePodName.get(),
+                imuName.get(),
+                logoDirection.get(),
+                usbDirection.get()
+        );
+
+        Double forwardTicksPerInchResult = runOpMode(new TwoWheelForwardResolution(values, distance.get()));
+        Double strafeTicksPerInchResult = runOpMode(new TwoWheelStrafeResolution(values, distance.get()));
+
+        if (forwardTicksPerInchResult == null || strafeTicksPerInchResult == null
+                || forwardTicksPerInchResult == 0.0 || strafeTicksPerInchResult == 0.0) {
+            abort("Encoder resolution measurement was zero. Complete both pushes before pressing Stop.");
+            return;
+        }
+
+        double forwardTicksPerInch = forwardTicksPerInchResult;
+        double strafeTicksPerInch = strafeTicksPerInchResult;
+
+        double forwardTicksToInches = 1.0 / forwardTicksPerInch;
+        double strafeTicksToInches = 1.0 / strafeTicksPerInch;
+
+        boolean forwardPodReversed = runOpMode(
+                new TwoWheelForwardDirection(values, forwardTicksToInches, strafeTicksToInches)
+        );
+
+        boolean strafePodReversed = runOpMode(
+                new TwoWheelStrafeDirection(values, forwardTicksToInches, strafeTicksToInches)
+        );
+
+        List<Double> offsets = runOpMode(
+                new TwoWheelOffsets(
+                        values,
+                        forwardTicksToInches,
+                        strafeTicksToInches,
+                        forwardPodReversed,
+                        strafePodReversed
+                )
+        );
+
+        result("xPodName", values.forwardPodName);
+        result("yPodName", values.strafePodName);
+        result("imuName", values.imuName);
+        result("logoDirection", values.logoDirection);
+        result("usbDirection", values.usbDirection);
+        result("forwardTicksPerInch", forwardTicksPerInch);
+        result("strafeTicksPerInch", strafeTicksPerInch);
+        result("forwardTicksToInches", forwardTicksToInches);
+        result("strafeTicksToInches", strafeTicksToInches);
+        result("xPodDirection", forwardPodReversed ? "REVERSED" : "FORWARD");
+        result("yPodDirection", strafePodReversed ? "REVERSED" : "FORWARD");
+        result("xPodOffset", offsets.get(0));
+        result("yPodOffset", offsets.get(1));
+
+        code(Language.JAVA,
+                "public static TwoWheelConfig localizerConfig = new TwoWheelConfig(c -> {\n" +
+                        "    c.xPodName.set(\"" + values.forwardPodName + "\");\n" +
+                        "    c.yPodName.set(\"" + values.strafePodName + "\");\n" +
+                        "    c.imuName.set(\"" + values.imuName + "\");\n" +
+                        "    c.xPodOffset.set(" + offsets.get(0) + ");\n" +
+                        "    c.yPodOffset.set(" + offsets.get(1) + ");\n" +
+                        "    c.forwardTicksToInches.set(" + forwardTicksToInches + ");\n" +
+                        "    c.strafeTicksToInches.set(" + strafeTicksToInches + ");\n" +
+                        "    c.xPodDirection.set(" +
+                        (forwardPodReversed ? "Encoder.REVERSE" : "Encoder.FORWARD") +
+                        ");\n" +
+                        "    c.yPodDirection.set(" +
+                        (strafePodReversed ? "Encoder.REVERSE" : "Encoder.FORWARD") +
+                        ");\n" +
+                        "    c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(\n" +
+                        "            RevHubOrientationOnRobot.LogoFacingDirection." + values.logoDirection.name() + ",\n" +
+                        "            RevHubOrientationOnRobot.UsbFacingDirection." + values.usbDirection.name() + "\n" +
+                        "    )));\n" +
+                        "});"
+        );
+    }
+
+    static TwoWheelConfig config(
+            TwoWheelSetup values,
+            double forwardTicksToInches,
+            double strafeTicksToInches,
+            double xPodDirection,
+            double yPodDirection,
+            double xPodOffset,
+            double yPodOffset
+    ) {
+        return new TwoWheelConfig(c -> {
+            c.xPodName.set(values.forwardPodName);
+            c.yPodName.set(values.strafePodName);
+            c.imuName.set(values.imuName);
+            c.xPodOffset.set(xPodOffset);
+            c.yPodOffset.set(yPodOffset);
+            c.forwardTicksToInches.set(forwardTicksToInches);
+            c.strafeTicksToInches.set(strafeTicksToInches);
+            c.xPodDirection.set(xPodDirection);
+            c.yPodDirection.set(yPodDirection);
+            c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(values.logoDirection, values.usbDirection)));
+        });
+    }
+}
+
+class TwoWheelSetup {
+
+    String forwardPodName;
+    String strafePodName;
+    String imuName;
+    RevHubOrientationOnRobot.LogoFacingDirection logoDirection;
+    RevHubOrientationOnRobot.UsbFacingDirection usbDirection;
+
+    TwoWheelSetup(
+            String forwardPodName,
+            String strafePodName,
+            String imuName,
+            RevHubOrientationOnRobot.LogoFacingDirection logoDirection,
+            RevHubOrientationOnRobot.UsbFacingDirection usbDirection
+    ) {
+        this.forwardPodName = forwardPodName;
+        this.strafePodName = strafePodName;
+        this.imuName = imuName;
+        this.logoDirection = logoDirection;
+        this.usbDirection = usbDirection;
+    }
+}
+
+class TwoWheelForwardResolution extends TuningOpMode<Double> {
+
+    TwoWheelSetup values;
+    double distance;
+
+    TwoWheelForwardResolution(TwoWheelSetup values, double distance) {
+        super(
+                "Forward Encoder Resolution Identification",
+                "Push your robot forward " + distance + " inches exactly and then stop the Opmode",
+                true
+        );
+        this.values = values;
+        this.distance = distance;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        TwoWheelConfig config = TwoWheelTuner.config(
+                values,
+                1.0,
+                1.0,
+                Encoder.FORWARD,
+                Encoder.FORWARD,
+                0.0,
+                0.0
+        );
+
+        TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        Pose position = null;
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+            position = localizer.pose();
+            telemetry.addData("heading", localizer.pose().heading());
+            telemetry.addData("pose", localizer.pose());
+            telemetry.update();
+        }
+
+        if (position == null || position.x() == 0.0) {
+            return null;
+        }
+        return Math.abs(position.x() / distance);
+    }
+}
+
+class TwoWheelStrafeResolution extends TuningOpMode<Double> {
+
+    TwoWheelSetup values;
+    double distance;
+
+    TwoWheelStrafeResolution(TwoWheelSetup values, double distance) {
+        super(
+                "Strafe Encoder Resolution Identification",
+                "Push your robot left " + distance + " inches exactly and then stop the Opmode",
+                true
+        );
+        this.values = values;
+        this.distance = distance;
+    }
+
+    @Override
+    protected Double runTuningOpMode() {
+        TwoWheelConfig config = TwoWheelTuner.config(
+                values,
+                1.0,
+                1.0,
+                Encoder.FORWARD,
+                Encoder.FORWARD,
+                0.0,
+                0.0
+        );
+
+        TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+        Pose position = null;
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+            position = localizer.pose();
+        }
+
+        if (position == null || position.y() == 0.0) {
+            return null;
+        }
+        return Math.abs(position.y() / distance);
+    }
+}
+
+class TwoWheelForwardDirection extends TuningOpMode<Boolean> {
+
+    TwoWheelSetup values;
+    double forwardTicksToInches;
+    double strafeTicksToInches;
+
+    TwoWheelForwardDirection(TwoWheelSetup values, double forwardTicksToInches, double strafeTicksToInches) {
+        super(
+                "Forward Direction Identification",
+                "Determines if your forward pod needs to be reversed.\n"
+                        + "Push your robot forward and then stop the Opmode",
+                true
+        );
+        this.values = values;
+        this.forwardTicksToInches = forwardTicksToInches;
+        this.strafeTicksToInches = strafeTicksToInches;
+    }
+
+    @Override
+    protected Boolean runTuningOpMode() {
+        TwoWheelConfig config = TwoWheelTuner.config(
+                values,
+                forwardTicksToInches,
+                strafeTicksToInches,
+                Encoder.FORWARD,
+                Encoder.FORWARD,
+                0.0,
+                0.0
+        );
+
+        TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+        }
+
+        return localizer.pose().x() < 0;
+    }
+}
+
+class TwoWheelStrafeDirection extends TuningOpMode<Boolean> {
+
+    TwoWheelSetup values;
+    double forwardTicksToInches;
+    double strafeTicksToInches;
+
+    TwoWheelStrafeDirection(TwoWheelSetup values, double forwardTicksToInches, double strafeTicksToInches) {
+        super(
+                "Strafe Direction Identification",
+                "Determines if your strafe pod needs to be reversed.\n"
+                        + "Push your robot left and then stop the Opmode",
+                true
+        );
+        this.values = values;
+        this.forwardTicksToInches = forwardTicksToInches;
+        this.strafeTicksToInches = strafeTicksToInches;
+    }
+
+    @Override
+    protected Boolean runTuningOpMode() {
+        TwoWheelConfig config = TwoWheelTuner.config(
+                values,
+                forwardTicksToInches,
+                strafeTicksToInches,
+                Encoder.FORWARD,
+                Encoder.FORWARD,
+                0.0,
+                0.0
+        );
+
+        TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
+        localizer.setPose(new Pose(0, 0));
+
+        waitForStart();
+        while (!isStopRequested()) {
+            localizer.update();
+        }
+
+        return localizer.pose().y() < 0;
+    }
+}
+
+class TwoWheelOffsets extends TuningOpMode<List<Double>> {
+
+    TwoWheelSetup values;
+    double forwardTicksToInches;
+    double strafeTicksToInches;
+    boolean forwardPodReversed;
+    boolean strafePodReversed;
+    Pose previous = Pose.zero();
+
+    TwoWheelOffsets(
+            TwoWheelSetup values,
+            double forwardTicksToInches,
+            double strafeTicksToInches,
+            boolean forwardPodReversed,
+            boolean strafePodReversed
+    ) {
+        super(
+                "Two Wheel Offset Identification",
+                "Automatically identifies the offsets for your Two Wheel localizer.\n"
+                        + "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode",
+                true
+        );
+        this.values = values;
+        this.forwardTicksToInches = forwardTicksToInches;
+        this.strafeTicksToInches = strafeTicksToInches;
+        this.forwardPodReversed = forwardPodReversed;
+        this.strafePodReversed = strafePodReversed;
+    }
+
+    @Override
+    protected List<Double> runTuningOpMode() {
+        TwoWheelConfig config = TwoWheelTuner.config(
+                values,
+                forwardTicksToInches,
+                strafeTicksToInches,
+                forwardPodReversed ? Encoder.REVERSE : Encoder.FORWARD,
+                strafePodReversed ? Encoder.REVERSE : Encoder.FORWARD,
+                0.0,
+                0.0
+        );
+
+        TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
+        localizer.setPose(Pose.zero());
+        localizer.update();
+
+        waitForStart();
+
+        localizer.setPose(Pose.zero());
+
+        while (!isStopRequested()) {
+            previous = localizer.pose();
+            localizer.update();
+
+            telemetry.addData("heading", localizer.pose().heading());
+            telemetry.addData("pose", localizer.pose());
+            telemetry.addData("previous", previous);
+            telemetry.update();
+        }
+
+        if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) {
+            previous = localizer.pose();
+        }
+
+        return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0));
+    }
+}

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

@@ -1,19 +0,0 @@
-package org.firstinspires.ftc.teamcode.pedroPathing;
-
-import com.pedropathing.follower.Follower;
-import com.pedropathing.follower.FollowerConstants;
-import com.pedropathing.ftc.FollowerBuilder;
-import com.pedropathing.paths.PathConstraints;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-public class Constants {
-    public static FollowerConstants followerConstants = new FollowerConstants();
-
-    public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
-
-    public static Follower createFollower(HardwareMap hardwareMap) {
-        return new FollowerBuilder(followerConstants, hardwareMap)
-                .pathConstraints(pathConstraints)
-                .build();
-    }
-}

build.dependencies.gradle

@@ -1,23 +1,22 @@
 repositories {
     mavenCentral()
     google() // Needed for androidx
+    maven { url 'https://repo.dairy.foundation/releases/'}
     maven { url = "https://mymaven.bylazar.com/releases" }
 }
 
 dependencies {
-    implementation 'org.firstinspires.ftc:Inspection:11.2.1'
-    implementation 'org.firstinspires.ftc:Blocks:11.2.1'
-    //noinspection Aligned16KB
-    implementation 'org.firstinspires.ftc:RobotCore:11.2.1'
-    implementation 'org.firstinspires.ftc:RobotServer:11.2.1'
-    implementation 'org.firstinspires.ftc:OnBotJava:11.2.1'
-    implementation 'org.firstinspires.ftc:Hardware:11.2.1'
-    implementation 'org.firstinspires.ftc:FtcCommon:11.2.1'
-    implementation 'org.firstinspires.ftc:Vision:11.2.1'
-    //noinspection GradleDependency
+    implementation 'org.firstinspires.ftc:Inspection:12.0.0'
+    implementation 'org.firstinspires.ftc:Blocks:12.0.0'
+    implementation 'org.firstinspires.ftc:RobotCore:12.0.0'
+    implementation 'org.firstinspires.ftc:RobotServer:12.0.0'
+    implementation 'org.firstinspires.ftc:OnBotJava:12.0.0'
+    implementation 'org.firstinspires.ftc:Hardware:12.0.0'
+    implementation 'org.firstinspires.ftc:FtcCommon:12.0.0'
+    implementation 'org.firstinspires.ftc:Vision:12.0.0'
     implementation 'androidx.appcompat:appcompat:1.2.0'
-    implementation 'com.pedropathing:ftc:2.1.2'
-    implementation 'com.pedropathing:telemetry:1.0.0'
-    implementation 'com.bylazar:fullpanels:1.0.12'
-}
 
+    implementation 'com.bylazar:fullpanels:1.0.12'
+    implementation 'com.pedropathing:revhub:3.0.0'
+    implementation 'com.pedropathing:tuning:1.0.0'
+}