From 6830a8240318d27ba140610ef0e7f55a0e359c4e Mon Sep 17 00:00:00 2001 From: Logan-Nash Date: Fri, 25 Oct 2024 11:54:29 -0400 Subject: [PATCH] Added fixes to external odometry processors. Signed-off-by: Logan-Nash --- .../localization/localizers/OTOSLocalizer.java | 8 +++++++- .../localization/localizers/PinpointLocalizer.java | 9 +++++++-- 2 files changed, 14 insertions(+), 3 deletions(-) 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())); } /**