optimized OTOSLocalizer thanks to j5155

This commit is contained in:
Anyi Lin
2024-10-10 03:46:24 -04:00
parent ce3ae6c03b
commit e0a96df549

View File

@ -41,6 +41,9 @@ public class OTOSLocalizer extends Localizer {
private HardwareMap hardwareMap; private HardwareMap hardwareMap;
private Pose startPose; private Pose startPose;
private SparkFunOTOS otos; private SparkFunOTOS otos;
private SparkFunOTOS.Pose2D otosPose;
private SparkFunOTOS.Pose2D otosVel;
private SparkFunOTOS.Pose2D otosAcc;
private double previousHeading; private double previousHeading;
private double totalHeading; private double totalHeading;
@ -89,6 +92,9 @@ public class OTOSLocalizer extends Localizer {
otos.resetTracking(); otos.resetTracking();
setStartPose(setStartPose); setStartPose(setStartPose);
otosPose = new SparkFunOTOS.Pose2D();
otosVel = new SparkFunOTOS.Pose2D();
otosAcc = new SparkFunOTOS.Pose2D();
totalHeading = 0; totalHeading = 0;
previousHeading = startPose.getHeading(); previousHeading = startPose.getHeading();
@ -102,8 +108,7 @@ public class OTOSLocalizer extends Localizer {
*/ */
@Override @Override
public Pose getPose() { public Pose getPose() {
SparkFunOTOS.Pose2D pose = otos.getPosition(); return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h));
return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h));
} }
/** /**
@ -113,8 +118,7 @@ public class OTOSLocalizer extends Localizer {
*/ */
@Override @Override
public Pose getVelocity() { public Pose getVelocity() {
SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity(); return new Pose(otosVel.x, otosVel.y, otosVel.h);
return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h);
} }
/** /**
@ -156,8 +160,9 @@ public class OTOSLocalizer extends Localizer {
*/ */
@Override @Override
public void update() { public void update() {
totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading); otos.getPosVelAcc(otosPose,otosVel,otosAcc);
previousHeading = otos.getPosition().h; totalHeading += MathFunctions.getSmallestAngleDifference(otosPose.h, previousHeading);
previousHeading = otosPose.h;
} }
/** /**