Use Pigeon native function.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
BroncBotz3481 · Feb 7, 2024 · 990f442 · 990f442
1 parent e7f94fb
commit 990f442
Showing 1 changed file with 1 addition and 25 deletions.
diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java
@@ -106,31 +106,7 @@ public void setInverted(boolean invertIMU)
   @Override
   public Rotation3d getRawRotation3d()
   {
-    // TODO: Switch to suppliers.
-    StatusSignal<Double> w = imu.getQuatW();
-    StatusSignal<Double> x = imu.getQuatX();
-    StatusSignal<Double> y = imu.getQuatY();
-    StatusSignal<Double> z = imu.getQuatZ();
-    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()));
+    Rotation3d reading = imu.getRotation3d();
     return invertedIMU ? reading.unaryMinus() : reading;
   }