From fd5d6b716b3cc643e93370990ecfc836d15d2c5e Mon Sep 17 00:00:00 2001
From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com>
Date: Fri, 6 Dec 2024 09:17:29 -0800
Subject: [PATCH 1/5] Update PinpointLocalizer.java

---
 .../localizers/PinpointLocalizer.java         | 29 ++++++++++++-------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
index b0ef6bc2..f6ee7333 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
@@ -79,13 +79,11 @@
 //        //TODO: Set encoder directions
 //        odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);
 //
-//        odo.resetPosAndIMU();
-//
+//        resetPinpoint();;
+
 //        setStartPose(setStartPose);
 //        totalHeading = 0;
 //        previousHeading = setStartPose.getHeading();
-//
-//        resetPinpoint();
 //    }
 //
 //    /**
@@ -142,7 +140,6 @@
 //     */
 //    @Override
 //    public void setPose(Pose setPose) {
-//        resetPinpoint();
 //        odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading()));
 //    }
 //
@@ -195,17 +192,29 @@
 //    }
 //
 //    /**
-//     * This resets the IMU.
+//     * This resets the IMU. Note: This does not change the estimated heading orientation.
 //     */
 //    @Override
-//    public void resetIMU() {
-//    odo.recalibrateIMU();
+//    public void resetIMU() throws InterruptedException {
+//        odo.recalibrateIMU();
+//
+//        try {
+//            Thread.sleep(300);
+//        } catch (InterruptedException e) {
+//            throw new RuntimeException(e);
+//        }
 //    }
 //
 //    /**
-//     * This resets the OTOS.
+//     * This resets the pinpoint.
 //     */
-//    public void resetPinpoint(){
+//    private void resetPinpoint() {
 //        odo.resetPosAndIMU();
+//
+//        try {
+//            Thread.sleep(300);
+//        } catch (InterruptedException e) {
+//            throw new RuntimeException(e);
+//        }
 //    }
 //}

From 7bc4dbbd341078e25f0499ff31641a00367e540a Mon Sep 17 00:00:00 2001
From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com>
Date: Fri, 6 Dec 2024 09:18:37 -0800
Subject: [PATCH 2/5] Update PoseUpdater.java

---
 .../ftc/teamcode/pedroPathing/localization/PoseUpdater.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
index 3daf54a2..b718320f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
@@ -351,7 +351,7 @@ public Localizer getLocalizer() {
     /**
      *
      */
-    public void resetIMU() {
+    public void resetIMU() throws InterruptedException {
         localizer.resetIMU();
     }
 }

From 8a55b00c42fe7b89aff23aef4efc52e21b42cb15 Mon Sep 17 00:00:00 2001
From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com>
Date: Fri, 6 Dec 2024 09:19:48 -0800
Subject: [PATCH 3/5] Update Localizer.java

---
 .../ftc/teamcode/pedroPathing/localization/Localizer.java       | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java
index 059ee318..64710ace 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java
@@ -92,7 +92,7 @@ public abstract class Localizer {
     /**
      * This resets the IMU of the localizer, if applicable.
      */
-    public abstract void resetIMU();
+    public abstract void resetIMU() throws InterruptedException;
 
     /**
      * This is overridden to return the IMU, if there is one.

From 7471de24d27fbe418080127e4169b2271edb817e Mon Sep 17 00:00:00 2001
From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com>
Date: Fri, 6 Dec 2024 09:20:27 -0800
Subject: [PATCH 4/5] Update Follower.java

---
 .../ftc/teamcode/pedroPathing/follower/Follower.java            | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
index 5089eb4c..23885ca8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
@@ -1019,7 +1019,7 @@ public DashboardPoseTracker getDashboardPoseTracker() {
     /**
      * This resets the IMU, if applicable.
      */
-    public void resetIMU() {
+    private void resetIMU() throws InterruptedException {
         poseUpdater.resetIMU();
     }
 }

From 9330c2221315f845986f7df4fcfd5daec072c7bb Mon Sep 17 00:00:00 2001
From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com>
Date: Fri, 6 Dec 2024 09:36:06 -0800
Subject: [PATCH 5/5] Update PinpointLocalizer.java

---
 .../pedroPathing/localization/localizers/PinpointLocalizer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
index f6ee7333..73272b88 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
@@ -105,7 +105,7 @@
 //    @Override
 //    public Pose getVelocity() {
 //        Pose2D pose = odo.getVelocity();
-//        return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS));
+//        return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), odo.getHeadingVelocity());
 //    }
 //
 //    /**