diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index e4e1f34..2d38fd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -108,7 +108,13 @@ public class OTOSLocalizer extends Localizer { */ @Override public Pose getPose() { - return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h)); + SparkFunOTOS.Pose2D rawPose = otos.getPosition(); + Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h); + + Vector vec = pose.getVector(); + vec.rotateVector(startPose.getHeading()); + + return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); } /** 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 151311b..c36a0f7 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 @@ -96,8 +96,13 @@ public class PinpointLocalizer extends Localizer { */ @Override public Pose getPose() { - Pose2D pose = odo.getPosition(); - return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); + Pose2D rawPose = odo.getPosition(); + Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); + + Vector vec = pose.getVector(); + vec.rotateVector(startPose.getHeading()); + + return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading())); } /**