From 6b601b9dd44ad82abc009525b30ad196dd98920e Mon Sep 17 00:00:00 2001
From: junkjunk123 <139065303+junkjunk123@users.noreply.github.com>
Date: Thu, 5 Dec 2024 18:08:59 -0800
Subject: [PATCH] Start position heading fix on TwoWheelPinpointIMU

---
 .../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 d2e03f31..bdf418ef 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
@@ -113,7 +113,7 @@ public TwoWheelPinpointIMULocalizer(HardwareMap map, Pose setStartPose) {
      */
     @Override
     public Pose getPose() {
-        return MathFunctions.addPoses(startPose, displacementPose);
+        return new Pose(startPose.getX()+displacementPose.getX(), startPose.getY()+displacementPose.getY(),displacementPose.getHeading());
     }
 
     /**