Changed status frame timeouts to static variables.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
BroncBotz3481 · Feb 7, 2024 · 74fb390 · 74fb390
1 parent 6ef7229
commit 74fb390
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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.
    */