From e90979d05e220c956482a9ad7ec479639c7d4a9d Mon Sep 17 00:00:00 2001
From: j5155 <54331556+j5155@users.noreply.github.com>
Date: Sat, 6 Jul 2024 15:27:01 -0800
Subject: [PATCH] read pos and vel at the same time, massively improving loop
 times

---
 TeamCode/build.gradle                                        | 2 +-
 .../org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java    | 5 +++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 7d0edb4624f2..0fa4dcb59fb1 100644
--- 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
index e7e1b7882af4..3dab3f0df781 100644
--- 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;