From e0a96df549b0cae6f97536532b06172d8061df9c Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 10 Oct 2024 03:46:24 -0400 Subject: [PATCH] optimized OTOSLocalizer thanks to j5155 --- .../localization/localizers/OTOSLocalizer.java | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 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 6a0bb382..e4e1f34a 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 @@ -41,6 +41,9 @@ public class OTOSLocalizer extends Localizer { private HardwareMap hardwareMap; private Pose startPose; private SparkFunOTOS otos; + private SparkFunOTOS.Pose2D otosPose; + private SparkFunOTOS.Pose2D otosVel; + private SparkFunOTOS.Pose2D otosAcc; private double previousHeading; private double totalHeading; @@ -89,6 +92,9 @@ public OTOSLocalizer(HardwareMap map, Pose setStartPose) { otos.resetTracking(); setStartPose(setStartPose); + otosPose = new SparkFunOTOS.Pose2D(); + otosVel = new SparkFunOTOS.Pose2D(); + otosAcc = new SparkFunOTOS.Pose2D(); totalHeading = 0; previousHeading = startPose.getHeading(); @@ -102,8 +108,7 @@ public OTOSLocalizer(HardwareMap map, Pose setStartPose) { */ @Override public Pose getPose() { - SparkFunOTOS.Pose2D pose = otos.getPosition(); - return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h)); + return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h)); } /** @@ -113,8 +118,7 @@ public Pose getPose() { */ @Override public Pose getVelocity() { - SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity(); - return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h); + return new Pose(otosVel.x, otosVel.y, otosVel.h); } /** @@ -156,8 +160,9 @@ public void setPose(Pose setPose) { */ @Override public void update() { - totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading); - previousHeading = otos.getPosition().h; + otos.getPosVelAcc(otosPose,otosVel,otosAcc); + totalHeading += MathFunctions.getSmallestAngleDifference(otosPose.h, previousHeading); + previousHeading = otosPose.h; } /**