From 74fb390248a1c486066b9c31fdf7aebd9c603c02 Mon Sep 17 00:00:00 2001
From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com>
Date: Wed, 7 Feb 2024 12:04:10 -0600
Subject: [PATCH] Changed status frame timeouts to static variables.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
---
 .../swervelib/encoders/CANCoderSwerve.java    |  2 +-
 .../java/swervelib/imu/Pigeon2Swerve.java     | 27 +++++++++++++++----
 .../java/swervelib/motors/TalonFXSwerve.java  |  2 +-
 3 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java
index da6c4653..24ca2013 100644
--- a/src/main/java/swervelib/encoders/CANCoderSwerve.java
+++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java
@@ -20,7 +20,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
   /**
    * Wait time for status frames to show up.
    */
-  private final double   STATUS_TIMEOUT_SECONDS = 0.02;
+  public static double   STATUS_TIMEOUT_SECONDS = 0.02;
   /**
    * CANCoder with WPILib sendable and support.
    */
diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java
index 0a3e5526..4db3ea1c 100644
--- a/src/main/java/swervelib/imu/Pigeon2Swerve.java
+++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java
@@ -1,5 +1,6 @@
 package swervelib.imu;
 
+import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.Pigeon2Configurator;
@@ -19,7 +20,7 @@ public class Pigeon2Swerve extends SwerveIMU
   /**
    * Wait time for status frames to show up.
    */
-  private final double     STATUS_TIMEOUT_SECONDS = 0.02;
+  public static double     STATUS_TIMEOUT_SECONDS = 0.04;
   /**
    * Pigeon2 IMU device.
    */
@@ -110,10 +111,26 @@ public Rotation3d getRawRotation3d()
     StatusSignal<Double> x = imu.getQuatX();
     StatusSignal<Double> y = imu.getQuatY();
     StatusSignal<Double> z = imu.getQuatZ();
-    Rotation3d reading = new Rotation3d(new Quaternion(w.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
-                                                       x.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
-                                                       y.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
-                                                       z.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()));
+    if(w.getStatus() != StatusCode.OK)
+    {
+      w = w.waitForUpdate(STATUS_TIMEOUT_SECONDS);
+    }
+    if(x.getStatus() != StatusCode.OK)
+    {
+      x = x.waitForUpdate(STATUS_TIMEOUT_SECONDS);
+    }
+    if(y.getStatus() != StatusCode.OK)
+    {
+      y = y.waitForUpdate(STATUS_TIMEOUT_SECONDS);
+    }
+    if(z.getStatus() != StatusCode.OK)
+    {
+      z = z.waitForUpdate(STATUS_TIMEOUT_SECONDS);
+    }
+    Rotation3d reading = new Rotation3d(new Quaternion(w.getValue(),
+                                                       x.getValue(),
+                                                       y.getValue(),
+                                                       z.getValue()));
     return invertedIMU ? reading.unaryMinus() : reading;
   }
 
diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java
index 9c44c72a..b2d2eec7 100644
--- a/src/main/java/swervelib/motors/TalonFXSwerve.java
+++ b/src/main/java/swervelib/motors/TalonFXSwerve.java
@@ -36,7 +36,7 @@ public class TalonFXSwerve extends SwerveMotor
   /**
    * Wait time for status frames to show up.
    */
-  private final double               STATUS_TIMEOUT_SECONDS = 0.02;
+  public static double               STATUS_TIMEOUT_SECONDS = 0.02;
   /**
    * TalonFX motor controller.
    */