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] 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 b0ef6bc..f6ee733 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);
+//        }
 //    }
 //}