From 38856c90dcc33fdbfc048839e10fbddaa6508c9f Mon Sep 17 00:00:00 2001
From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com>
Date: Thu, 19 Dec 2024 12:48:00 -0800
Subject: [PATCH 1/2] Update TwoWheelPinpointIMULocalizer.java

---
 .../TwoWheelPinpointIMULocalizer.java         | 23 ++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java
index bdf418ef..9b8629b9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java
@@ -103,6 +103,7 @@ public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) {
         deltaTimeNano = 1;
         displacementPose = new Pose();
         currentVelocity = new Pose();
+        previousHeading = startPose.getHeading();
         deltaRadians = 0;
     }
 
@@ -293,8 +294,28 @@ public double getTurningMultiplier() {
      * This resets the IMU.
      */
 
-    public void resetIMU() {
+    @Override
+    public void resetIMU() throws InterruptedException {
+        pinpoint.recalibrateIMU();
+
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+
+    /**
+     * This resets the pinpoint.
+     */
+    private void resetPinpoint() {
         pinpoint.resetPosAndIMU();
+
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
     }
 }
 

From db79dc38dcc6aed64ad2c97ca2cdeba8bd6e632b Mon Sep 17 00:00:00 2001
From: Havish Sripada <139065303+junkjunk123@users.noreply.github.com>
Date: Thu, 19 Dec 2024 12:52:26 -0800
Subject: [PATCH 2/2] Update TwoWheelPinpointIMULocalizer.java

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

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java
index 9b8629b9..87849b98 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java
@@ -308,7 +308,7 @@ public void resetIMU() throws InterruptedException {
     /**
      * This resets the pinpoint.
      */
-    private void resetPinpoint() {
+    private void resetPinpoint() throws InterruptedException{
         pinpoint.resetPosAndIMU();
 
         try {