Start position heading fix on TwoWheelPinpointIMU

2024-12-05 18:08:59 -08:00
parent 58fd8d4a56
commit 6b601b9dd4
1 changed files with 1 additions and 1 deletions
--- 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 class TwoWheelPinpointIMULocalizer extends Localizer {
     */
    @Override
    public Pose getPose() {
-        return MathFunctions.addPoses(startPose, displacementPose);
+        return new Pose(startPose.getX()+displacementPose.getX(), startPose.getY()+displacementPose.getY(),displacementPose.getHeading());
    }

    /**