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 6a0bb38..e4e1f34 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 class OTOSLocalizer extends Localizer { 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 class OTOSLocalizer extends Localizer { */ @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 class OTOSLocalizer extends Localizer { */ @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 class OTOSLocalizer extends Localizer { */ @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; } /**