From 4e18dae336ce79df1290463dbefc4cf65abab1ca Mon Sep 17 00:00:00 2001 From: TBHGodPro Date: Sat, 26 Oct 2024 10:57:10 -0500 Subject: [PATCH] Improve OTOSLocalizer and PinpointLocalizer performance --- .../localization/localizers/OTOSLocalizer.java | 5 +---- .../localizers/PinpointLocalizer.java | 5 +---- .../pathGeneration/MathFunctions.java | 16 ++++++++++++++++ 3 files changed, 18 insertions(+), 8 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 2d38fd1c..463caa0e 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 @@ -111,10 +111,7 @@ public Pose getPose() { 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())); + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** 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 c36a0f78..ef0b1333 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 @@ -99,10 +99,7 @@ public Pose getPose() { 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())); + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index 0edfbfd4..b43c6ffd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -172,6 +172,22 @@ public static Pose subtractPoses(Pose one, Pose two) { return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); } + /** + * This rotates the given pose by the given theta, + * + * @param pose the Pose to rotate. + * @param theta the angle to rotate by. + * @param rotateHeading whether to adjust the Pose heading too. + * @return the rotated Pose. + */ + public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) { + double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta); + double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta); + double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading(); + + return new Pose(x, y, heading); + } + /** * This multiplies a Point by a scalar and returns the result as a Point *