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 d2e03f3..bdf418e 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 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()); } /**