From b7553162b259f40a9d796734ad81e7caa048e115 Mon Sep 17 00:00:00 2001
From: Josh Kelley <josh9kelley@gmail.com>
Date: Sun, 3 Aug 2025 16:27:10 -0400
Subject: [PATCH 1/2] Cache setPosition() value if DeviceStatus is not READY

---
 .../ftc/teamcode/GoBildaPinpointDriver.java   | 24 ++++++++++++++-----
 1 file changed, 18 insertions(+), 6 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java
index 3781542c9bc6..1fa7adf8f86c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java
@@ -47,7 +47,7 @@
         name = "goBILDA® Pinpoint Odometry Computer",
         xmlTag = "goBILDAPinpoint",
         description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)"
-        )
+)
 
 public class GoBildaPinpointDriver extends I2cDeviceSynchDevice<I2cDeviceSynchSimple> {
 
@@ -65,6 +65,8 @@ public class GoBildaPinpointDriver extends I2cDeviceSynchDevice<I2cDeviceSynchSi
     private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod
     private static final float goBILDA_4_BAR_POD    = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod
 
+    private static volatile Pose2D cachedSetPositionValue = null;
+
     //i2c address of the device
     public static final byte DEFAULT_ADDRESS = 0x31;
 
@@ -157,7 +159,7 @@ public enum ReadData {
 
 
     /** Writes an int to the i2c device
-    @param reg the register to write the int to
+     @param reg the register to write the int to
      @param i the integer to write to the register
      */
     private void writeInt(final Register reg, int i){
@@ -245,6 +247,10 @@ private DeviceStatus lookupStatus (int s){
             return DeviceStatus.FAULT_IMU_RUNAWAY;
         }
         if ((s & DeviceStatus.READY.status) != 0){
+            if(cachedSetPositionValue != null){
+                setPosition(cachedSetPositionValue);
+                cachedSetPositionValue = null;
+            }
             return DeviceStatus.READY;
         }
         if ((s & DeviceStatus.FAULT_BAD_READ.status) != 0){
@@ -334,6 +340,7 @@ public void update(){
         yVelocity    = isVelocityCorrupt(oldVelY, yVelocity, velocityThreshold);
         hVelocity    = isVelocityCorrupt(oldVelH, hVelocity, headingVelocityThreshold);
 
+        getDeviceStatus();
     }
 
     /**
@@ -491,10 +498,15 @@ public void setYawScalar(double yawOffset){
      * @param pos a Pose2D describing the robot's new position.
      */
     public Pose2D setPosition(Pose2D pos){
-        writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN)));
-        writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN)));
-        writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN)));
-        return pos;
+        if(getDeviceStatus() != DeviceStatus.READY){
+            cachedSetPositionValue = pos;
+            return getPosition();
+        }else {
+            writeByteArray(Register.X_POSITION, (floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN)));
+            writeByteArray(Register.Y_POSITION, (floatToByteArray((float) pos.getY(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN)));
+            writeByteArray(Register.H_ORIENTATION, (floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS), ByteOrder.LITTLE_ENDIAN)));
+            return pos;
+        }
     }
 
     /**

From a82be5a39e64885f043a44ffe095466322717f72 Mon Sep 17 00:00:00 2001
From: Josh Kelley <josh9kelley@gmail.com>
Date: Sun, 3 Aug 2025 16:41:52 -0400
Subject: [PATCH 2/2] Fix typo

---
 .../firstinspires/ftc/teamcode/GoBildaPinpointDriver.java   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java
index 1fa7adf8f86c..c069d23d2d19 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java
@@ -513,7 +513,7 @@ public Pose2D setPosition(Pose2D pos){
      * Send a position that the Pinpoint should use to track your robot relative to.
      * You can use this to update the estimated position of your robot with new external
      * sensor data, or to run a robot in field coordinates.
-     * @param posX the new X position you'd like the Pinpoint to track your robot relive to.
+     * @param posX the new X position you'd like the Pinpoint to track your robot relative to.
      * @param distanceUnit the unit for posX
      */
     public void setPosX(double posX, DistanceUnit distanceUnit){
@@ -524,7 +524,7 @@ public void setPosX(double posX, DistanceUnit distanceUnit){
      * Send a position that the Pinpoint should use to track your robot relative to.
      * You can use this to update the estimated position of your robot with new external
      * sensor data, or to run a robot in field coordinates.
-     * @param posY the new Y position you'd like the Pinpoint to track your robot relive to.
+     * @param posY the new Y position you'd like the Pinpoint to track your robot relative to.
      * @param distanceUnit the unit for posY
      */
     public void setPosY(double posY, DistanceUnit distanceUnit){
@@ -535,7 +535,7 @@ public void setPosY(double posY, DistanceUnit distanceUnit){
      * Send a heading that the Pinpoint should use to track your robot relative to.
      * You can use this to update the estimated position of your robot with new external
      * sensor data, or to run a robot in field coordinates.
-     * @param heading the new heading you'd like the Pinpoint to track your robot relive to.
+     * @param heading the new heading you'd like the Pinpoint to track your robot relative to.
      * @param angleUnit Radians or Degrees
      */
     public void setHeading(double heading, AngleUnit angleUnit){