Merge pull request #26 from junkjunk123/patch-1

Pedro-Pathing · Dec 19, 2024 · a2cb798 · a2cb798
2 parents e4f0447 + db79dc3
commit a2cb798
Showing 1 changed file with 22 additions and 1 deletion.
diff --git a/...pires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java b/...pires/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() throws InterruptedException{
         pinpoint.resetPosAndIMU();
+
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
     }
 }