optimized OTOSLocalizer thanks to j5155
This commit is contained in:
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
Reference in New Issue
Block a user