SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

optimized OTOSLocalizer thanks to j5155

Browse Source
This commit is contained in:
Anyi Lin
2024-10-10 03:46:24 -04:00
parent ce3ae6c03b
commit e0a96df549
1 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
View File

@ -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;
}
/**