change gradle deps to include solvers lib

change robot to a final class for better standards
changed module name to work
2026-09-08 15:40:07 -05:00 · 2026-09-08 15:16:49 -05:00 · 2026-09-03 17:34:32 -05:00 · 2026-09-03 17:30:36 -05:00
24 changed files with 2073 additions and 3732 deletions
@@ -0,0 +1,27 @@
 <component name="ProjectRunConfigurationManager">
  <configuration default="false" name="Sloth Load" type="GradleRunConfiguration" factoryName="Gradle">
    <ExternalSystemSettings>
      <option name="executionName" />
      <option name="externalProjectPath" value="$PROJECT_DIR$" />
      <option name="externalSystemIdString" value="GRADLE" />
      <option name="scriptParameters" value="" />
      <option name="taskDescriptions">
        <list />
      </option>
      <option name="taskNames">
        <list>
          <option value=":TeamCode:deploySloth" />
        </list>
      </option>
      <option name="vmOptions" />
    </ExternalSystemSettings>
    <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
    <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
    <ExternalSystemDebugDisabled>false</ExternalSystemDebugDisabled>
    <DebugAllEnabled>false</DebugAllEnabled>
    <RunAsTest>false</RunAsTest>
    <GradleProfilingDisabled>false</GradleProfilingDisabled>
    <GradleCoverageDisabled>false</GradleCoverageDisabled>
    <method v="2" />
  </configuration>
 </component>
@@ -0,0 +1,69 @@
 <component name="ProjectRunConfigurationManager">
  <configuration default="false" name="TeamCode" type="AndroidRunConfigurationType" factoryName="Android App" activateToolWindowBeforeRun="false">
    <module name="biobuzz-ftc.TeamCode" />
    <option name="ANDROID_RUN_CONFIGURATION_SCHEMA_VERSION" value="1" />
    <option name="DEPLOY" value="true" />
    <option name="DEPLOY_APK_FROM_BUNDLE" value="false" />
    <option name="DEPLOY_AS_INSTANT" value="false" />
    <option name="ARTIFACT_NAME" value="" />
    <option name="PM_INSTALL_OPTIONS" value="" />
    <option name="ALL_USERS" value="false" />
    <option name="ALWAYS_INSTALL_WITH_PM" value="false" />
    <option name="ALLOW_ASSUME_VERIFIED" value="false" />
    <option name="CLEAR_APP_STORAGE" value="false" />
    <option name="DYNAMIC_FEATURES_DISABLED_LIST" value="" />
    <option name="ACTIVITY_EXTRA_FLAGS" value="" />
    <option name="MODE" value="default_activity" />
    <option name="RESTORE_ENABLED" value="false" />
    <option name="RESTORE_FILE" value="" />
    <option name="RESTORE_FRESH_INSTALL_ONLY" value="false" />
    <option name="CLEAR_LOGCAT" value="false" />
    <option name="SHOW_LOGCAT_AUTOMATICALLY" value="false" />
    <option name="TARGET_SELECTION_MODE" value="DEVICE_AND_SNAPSHOT_COMBO_BOX" />
    <option name="DEBUGGER_TYPE" value="Auto" />
    <Auto>
      <option name="USE_JAVA_AWARE_DEBUGGER" value="false" />
      <option name="SHOW_STATIC_VARS" value="true" />
      <option name="WORKING_DIR" value="" />
      <option name="TARGET_LOGGING_CHANNELS" value="lldb process:gdb-remote packets" />
      <option name="SHOW_OPTIMIZED_WARNING" value="true" />
      <option name="ATTACH_ON_WAIT_FOR_DEBUGGER" value="false" />
    </Auto>
    <Hybrid>
      <option name="USE_JAVA_AWARE_DEBUGGER" value="false" />
      <option name="SHOW_STATIC_VARS" value="true" />
      <option name="WORKING_DIR" value="" />
      <option name="TARGET_LOGGING_CHANNELS" value="lldb process:gdb-remote packets" />
      <option name="SHOW_OPTIMIZED_WARNING" value="true" />
      <option name="ATTACH_ON_WAIT_FOR_DEBUGGER" value="false" />
    </Hybrid>
    <Java>
      <option name="ATTACH_ON_WAIT_FOR_DEBUGGER" value="false" />
    </Java>
    <Native>
      <option name="USE_JAVA_AWARE_DEBUGGER" value="false" />
      <option name="SHOW_STATIC_VARS" value="true" />
      <option name="WORKING_DIR" value="" />
      <option name="TARGET_LOGGING_CHANNELS" value="lldb process:gdb-remote packets" />
      <option name="SHOW_OPTIMIZED_WARNING" value="true" />
      <option name="ATTACH_ON_WAIT_FOR_DEBUGGER" value="false" />
    </Native>
    <Profilers>
      <option name="ADVANCED_PROFILING_ENABLED" value="false" />
      <option name="STARTUP_PROFILING_ENABLED" value="false" />
      <option name="STARTUP_CPU_PROFILING_ENABLED" value="false" />
      <option name="STARTUP_CPU_PROFILING_CONFIGURATION_NAME" value="Java/Kotlin Method Trace" />
      <option name="STARTUP_NATIVE_MEMORY_PROFILING_ENABLED" value="false" />
      <option name="NATIVE_MEMORY_SAMPLE_RATE_BYTES" value="2048" />
    </Profilers>
    <option name="DEEP_LINK" value="" />
    <option name="ACTIVITY" value="" />
    <option name="ACTIVITY_CLASS" value="" />
    <option name="SEARCH_ACTIVITY_IN_GLOBAL_SCOPE" value="false" />
    <option name="SKIP_ACTIVITY_VALIDATION" value="false" />
    <method v="2">
      <option name="Gradle.BeforeRunTask" enabled="true" tasks="removeSlothRemote" externalProjectPath="$PROJECT_DIR$/TeamCode" vmOptions="" scriptParameters="" />
      <option name="Android.Gradle.BeforeRunTask" enabled="true" />
    </method>
  </configuration>
 </component>
@@ -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="12.0">
+          android:versionName="11.2.1">
  <uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />
@@ -38,10 +38,8 @@ 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;
@@ -152,12 +150,12 @@ public class ConceptAprilTag extends LinearOpMode {
        aprilTag = new AprilTagProcessor.Builder()
            // The following default settings are available to un-comment and edit as needed.
-            //.setDrawAxes(true) // Changed in V12.0
+            //.setDrawAxes(false)
            //.setDrawCubeProjection(false)
            //.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
@@ -222,25 +220,14 @@ public class ConceptAprilTag extends LinearOpMode {
        // Step through the list of detections and display info for each one.
        for (AprilTagDetection detection : currentDetections) {
-            if (detection instanceof AprilTagSingleDetection) {
+            if (detection.metadata != null) {
-                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
                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
@@ -36,10 +36,8 @@ 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;
@@ -47,8 +45,6 @@ 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
 *
@@ -82,10 +78,33 @@ 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() {
-        // See ConceptAprilTag.java for how to add a camera compatibility quirk
+        // 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);
        initAprilTag();
        // Wait for the DS start button to be touched.
@@ -146,25 +165,14 @@ public class ConceptAprilTagEasy extends LinearOpMode {
        // Step through the list of detections and display info for each one.
        for (AprilTagDetection detection : currentDetections) {
-            if (detection instanceof AprilTagSingleDetection) {
+            if (detection.metadata != null) {
-                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
                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
@@ -43,7 +43,6 @@ 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;
@@ -178,7 +177,7 @@ public class ConceptAprilTagLocalization extends LinearOpMode {
        aprilTag = new AprilTagProcessor.Builder()
                // The following default settings are available to un-comment and edit as needed.
-                //.setDrawAxes(true)  // changed in V12.0
+                //.setDrawAxes(false)
                //.setDrawCubeProjection(false)
                //.setDrawTagOutline(true)
                //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
@@ -248,23 +247,22 @@ public class ConceptAprilTagLocalization extends LinearOpMode {
        // Step through the list of detections and display info for each one.
        for (AprilTagDetection detection : currentDetections) {
-            if (detection instanceof AprilTagSingleDetection) {
+            if (detection.metadata != null) {
-                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
-
+                // Only use tags that don't have Obelisk in them
-                if (singleDet.metadata != null) {
+                if (!detection.metadata.name.contains("Obelisk")) {
-                    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)",
                    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("Robot PRY %6.1f %6.1f %6.1f  (deg)",
+                    telemetry.addLine(String.format("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
@@ -37,10 +37,8 @@ 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;
@@ -155,25 +153,14 @@ public class ConceptAprilTagSwitchableCameras extends LinearOpMode {
        // Step through the list of detections and display info for each one.
        for (AprilTagDetection detection : currentDetections) {
-            if (detection instanceof AprilTagSingleDetection) {
+            if (detection.metadata != null) {
-                AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection;
+                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
                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
@@ -39,32 +39,25 @@ 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 or AprilTag Cluster
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
 * 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/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera.
+ * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
 * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
 * https://ftc-docs.firstinspires.org/apriltag-detection-values
 *
- * For a single tag, the "Drive Target" is the center of the Tag.  For a cluster of tags, the "Drive Target" will be the
+ * 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
- * (0,0,0) ORIGIN of the cluster, which may have been positioned somewhere other than the center of the cluster in order
+ * driving towards the tag to achieve the desired distance.
 * 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.
 *
@@ -80,9 +73,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 Target centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 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 Target, so it approaches directly in front  of the tag.  (Use the Target Yaw to strafe 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 Target to get to the desired distance.  (Use TargetRange to drive the robot forward/backward)
+ * 3) Drive towards the Tag to get to the desired distance.  (Use Tag Range 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.
@@ -97,7 +90,7 @@ import java.util.concurrent.TimeUnit;
 public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
 {
    // Adjust these numbers to suit your robot.
-    final double DESIRED_DISTANCE = 30.0; //  this is how close the camera should get to the target (inches)
+    final double DESIRED_DISTANCE = 12.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.
@@ -106,31 +99,24 @@ 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_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_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_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 frontRightDrive = null;  //  Used to control the right front drive wheel
-    private DcMotor backLeftDrive = null;   //  Used to control the left back 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 final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
+    private static 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 static final int DESIRED_TAG_ID = -1;     // Choose 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)
@@ -166,57 +152,36 @@ 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 instanceof AprilTagSingleDetection) {
+                if (detection.metadata != null) {
-                    AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection;
+                    //  Check to see if we want to track towards this tag.
-
+                    if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
-                    // Look to see if we have size info on this tag.
+                        // Yes, we want to use this tag.
-                    if (singleDetection.metadata != null) {
+                        targetFound = true;
-                        //  Check to see if we want to track towards this tag.
+                        desiredTag = detection;
-                        if ((DESIRED_TAG_ID < 0) || (singleDetection.id == DESIRED_TAG_ID)) {
+                        break;  // don't look any further.
                            // 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 NOT in the library, so we don't have enough information to track to it.
+                        // This tag is in the library, but we do not want to track it right now.
-                        telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", singleDetection.id);
+                        telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
                    }
                } else {
-                    AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection;
+                    // 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);
                    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)", targetID, targetName);
+                telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
-                telemetry.addData("Range",  "%5.1f inches", targetRange);
+                telemetry.addData("Range",  "%5.1f inches", desiredTag.ftcPose.range);
-                telemetry.addData("Bearing","%3.0f degrees", targetBearing);
+                telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
-                telemetry.addData("Yaw","%3.0f degrees", targetYaw);
+                telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
            } else {
                telemetry.addData("\n>","Drive using joysticks to find valid target\n");
            }
@@ -225,9 +190,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      = targetRange - DESIRED_DISTANCE;
+                double  rangeError      = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
-                double  headingError    = targetBearing;
+                double  headingError    = desiredTag.ftcPose.bearing;
-                double  yawError        = targetYaw;
+                double  yawError        = desiredTag.ftcPose.yaw;
                // 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);
@@ -236,6 +201,7 @@ 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%.
@@ -252,8 +218,11 @@ 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) {
@@ -39,50 +39,44 @@ 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 or AprilTag Cluster
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
- * A "Cluster" is a group of Apriltags that share a common origin, and are identified by name.
+ * The code assumes a basic two-wheel (Tank) Robot Drivetrain
 * 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/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera.
+ * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
 * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
 * https://ftc-docs.firstinspires.org/apriltag-detection-values
 *
- * For a single tag, the "Drive Target" is the center of the Tag.  For a cluster of tags, the "Drive Target" will be the
+ * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance.
 * (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 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.
+ * 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 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.
+ * The motor directions must be set so a positive power goes forward on both wheels;
- * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default,
+ * This sample assumes that the default 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 & left/right.  The right stick will rotate the robot.
+ * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot.
- * Manually drive the robot until it displays Target data on the Driver Station.
+ * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel).
 *
 * 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:
+ *  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.)
+ *  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 Target to get to the desired distance.  (Use TargetRange to drive the robot forward/backward)
+ *  2) Drive towards the Tag to get to the desired distance.  (Use Tag Range 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.
@@ -95,7 +89,7 @@ import java.util.concurrent.TimeUnit;
 public class RobotAutoDriveToAprilTagTank extends LinearOpMode
 {
    // Adjust these numbers to suit your robot.
-    final double DESIRED_DISTANCE = 30.0; //  this is how close the camera should get to the target (inches)
+    final double DESIRED_DISTANCE = 12.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.
@@ -109,22 +103,15 @@ 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 final boolean USE_WEBCAM = true;  // Set true to use a webcam, or false for a phone camera
+    private static 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 static final int DESIRED_TAG_ID = -1;    // Choose 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
@@ -155,57 +142,35 @@ 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 instanceof AprilTagSingleDetection) {
+                if (detection.metadata != null) {
-                    AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection;
+                    //  Check to see if we want to track towards this tag.
-
+                    if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
                    // 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 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;
                        desiredTag = detection;
                        break;  // don't look any further.
                    } else {
                        // This tag is in the library, but we do not want to track it right now.
                        telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
                    }
                } 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)", targetID, targetName);
+                telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
-                telemetry.addData("Range",  "%5.1f inches", targetRange);
+                telemetry.addData("Range",  "%5.1f inches", desiredTag.ftcPose.range);
-                telemetry.addData("Bearing","%3.0f degrees", targetBearing);
+                telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
                telemetry.addData("Yaw","%3.0f degrees", targetYaw);
            } else {
                telemetry.addData("\n>","Drive using joysticks to find valid target\n");
            }
@@ -214,8 +179,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      = targetRange - DESIRED_DISTANCE;
+                double  rangeError   = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
-                double  headingError    = targetBearing;
+                double  headingError = desiredTag.ftcPose.bearing;
                // 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);
@@ -223,6 +188,7 @@ 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%.
@@ -238,7 +204,9 @@ 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) {
@@ -1,3 +1,15 @@
 buildscript {
    repositories {
        mavenCentral()
        maven {
            url "https://repo.dairy.foundation/releases"
        }
    }
    dependencies {
        classpath "dev.frozenmilk:Load:0.2.4"
    }
 }
 apply plugin: 'com.android.application'
 //
@@ -16,6 +28,8 @@ apply plugin: 'com.android.application'
 apply from: '../build.common.gradle'
 apply from: '../build.dependencies.gradle'
 apply plugin: 'dev.frozenmilk.sinister.sloth.load'
 android {
    namespace = 'org.firstinspires.ftc.teamcode'
@@ -24,9 +38,9 @@ android {
    }
 }
 dependencies {
    implementation project(':FtcRobotController')
-
+    implementation "dev.frozenmilk.sinister:Sloth:0.2.4"
    implementation "com.bylazar.sloth:fullpanels:0.2.4+1.0.12"
    implementation "org.solverslib:core:0.3.5" // core
 }
@@ -1,11 +0,0 @@
 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;
    }
 }
@@ -1,5 +0,0 @@
 package org.firstinspires.ftc.teamcode.pedro;
 public class Tuning {
    // Tuners go here
 }
@@ -1,96 +0,0 @@
 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;
    }
 }
@@ -1,210 +0,0 @@
 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);
    }
 }
@@ -1,350 +0,0 @@
 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));
    }
 }
@@ -1,256 +0,0 @@
 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));
    }
 }
@@ -1,327 +0,0 @@
 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;
    }
 }
@@ -1,328 +0,0 @@
 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;
        }
    }
 }
@@ -1,290 +0,0 @@
 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);
    }
 }
@@ -1,404 +0,0 @@
 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));
    }
 }
@@ -0,0 +1,19 @@
 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();
    }
 }
@@ -1,22 +1,23 @@
 repositories {
    mavenCentral()
-    google() // Needed for androidx
+    google()
-    maven { url 'https://repo.dairy.foundation/releases/'}
+    maven {url = "https://repo.dairy.foundation/releases" }
-    maven { url = "https://mymaven.bylazar.com/releases" }
+    maven {url = "https://mymaven.bylazar.com/releases" }
 }
 dependencies {
-    implementation 'org.firstinspires.ftc:Inspection:12.0.0'
+    implementation 'org.firstinspires.ftc:Inspection:11.2.1'
-    implementation 'org.firstinspires.ftc:Blocks:12.0.0'
+    implementation 'org.firstinspires.ftc:Blocks:11.2.1'
-    implementation 'org.firstinspires.ftc:RobotCore:12.0.0'
+    //noinspection Aligned16KB
-    implementation 'org.firstinspires.ftc:RobotServer:12.0.0'
+    implementation 'org.firstinspires.ftc:RobotCore:11.2.1'
-    implementation 'org.firstinspires.ftc:OnBotJava:12.0.0'
+    implementation 'org.firstinspires.ftc:RobotServer:11.2.1'
-    implementation 'org.firstinspires.ftc:Hardware:12.0.0'
+    implementation 'org.firstinspires.ftc:OnBotJava:11.2.1'
-    implementation 'org.firstinspires.ftc:FtcCommon:12.0.0'
+    implementation 'org.firstinspires.ftc:Hardware:11.2.1'
-    implementation 'org.firstinspires.ftc:Vision:12.0.0'
+    implementation 'org.firstinspires.ftc:FtcCommon:11.2.1'
    implementation 'org.firstinspires.ftc:Vision:11.2.1'
    //noinspection GradleDependency
    implementation 'androidx.appcompat:appcompat:1.2.0'
-
+    implementation 'com.pedropathing:ftc:2.1.2'
-    implementation 'com.bylazar:fullpanels:1.0.12'
+    implementation 'com.pedropathing:telemetry:1.0.0'
    implementation 'com.pedropathing:revhub:3.0.0'
    implementation 'com.pedropathing:tuning:1.0.0'
 }
Author	SHA1	Message	Date
KeshavAnandCode	202e107450	change gradle deps to include solvers lib	2026-09-08 15:40:07 -05:00
KeshavAnandCode	5d986c83b1	change robot to a final class for better standards	2026-09-08 15:16:49 -05:00
KeshavAnandCode	874cb0ba3b	changed module name to work	2026-09-03 17:34:32 -05:00
KeshavAnandCode	e04a16a692	added sloth deps and configs	2026-09-03 17:30:36 -05:00