From 659a22b33ede833fcc46dba9bb174568ad1a5473 Mon Sep 17 00:00:00 2001
From: Anyi Lin <anyo3223@gmail.com>
Date: Tue, 26 Nov 2024 04:54:23 -0500
Subject: [PATCH] fix total heading issue in PinpointLocalizer

---
 .../localization/localizers/PinpointLocalizer.java            | 4 ++--
 1 file changed, 2 insertions(+), 2 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 58c4746..e35875f 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
@@ -152,8 +152,8 @@ public class PinpointLocalizer extends Localizer {
     @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());
     }
 
     /**