fix total heading issue in PinpointLocalizer

Pedro-Pathing · Nov 26, 2024 · 659a22b · 659a22b
1 parent 6d4cd0f
commit 659a22b
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/...rg/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/...rg/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
@@ -152,8 +152,8 @@ public void setPose(Pose setPose) {
     @Override
     public void update() {
         odo.update();
-        totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(), previousHeading);
-        previousHeading = odo.getHeading();
+        totalHeading += MathFunctions.getSmallestAngleDifference(MathFunctions.normalizeAngle(odo.getHeading()), previousHeading);
+        previousHeading = MathFunctions.normalizeAngle(odo.getHeading());
     }
 
     /**