read pos and vel at the same time, massively improving loop times

wheelerrobotics · Jul 6, 2024 · e90979d · e90979d
1 parent 4cf4292
commit e90979d
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
@@ -36,7 +36,7 @@ dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
 
-    implementation "com.github.jdhs-ftc:road-runner-ftc-otos:7c00759558"
+    implementation "com.github.jdhs-ftc:road-runner-ftc-otos:22dd547404"
     implementation "com.acmerobotics.roadrunner:core:1.0.0-beta8"
     implementation "com.acmerobotics.roadrunner:actions:1.0.0-beta8"
     implementation "com.acmerobotics.dashboard:dashboard:0.4.15"

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java
@@ -102,8 +102,9 @@ public PoseVelocity2d updatePoseEstimate() {
             // the only alternative is to add getter and setters but that breaks compat
             otos.setPosition(RRPoseToOTOSPose(pose));
         }
-        SparkFunOTOS.Pose2D otosPose = otos.getPosition();
-        SparkFunOTOS.Pose2D otosVel = otos.getVelocity();
+        SparkFunOTOS.Pose2D[] posVelList = otos.getPosVelCorrected();
+        SparkFunOTOS.Pose2D otosPose = posVelList[0];
+        SparkFunOTOS.Pose2D otosVel = posVelList[1];
         pose = OTOSPoseToRRPose(otosPose);
         lastOtosPose = pose;