Updated IMU

swervelib/imu/PigeonSwerve.java

@@ -17,6 +17,10 @@ public class PigeonSwerve extends SwerveIMU
    * Pigeon v1 IMU device.
    */
   WPI_PigeonIMU imu;
+  /**
+   * Offset for the Pigeon.
+   */
+  private Rotation3d offset = new Rotation3d();
 
   /**
    * Generate the SwerveIMU for pigeon.
@@ -26,6 +30,7 @@ public class PigeonSwerve extends SwerveIMU
   public PigeonSwerve(int canid)
   {
     imu = new WPI_PigeonIMU(canid);
+    offset = new Rotation3d();
     SmartDashboard.putData(imu);
   }
 
@@ -48,25 +53,26 @@ public class PigeonSwerve extends SwerveIMU
   }
 
   /**
-   * Set the yaw in degrees.
+   * Set the gyro offset.
    *
-   * @param yaw Angle in degrees.
+   * @param offset gyro offset as a {@link Rotation3d}.
    */
-  @Override
-  public void setYaw(double yaw)
+  public void setOffset(Rotation3d offset)
   {
-    imu.setYaw(yaw);
+    offset = getRotation3d();
   }
 
   /**
-   * Fetch the yaw/pitch/roll from the IMU, inverts them all if SwerveIMU is inverted.
+   * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
-   * @param yprArray Array which will be filled with {yaw, pitch, roll} in degrees.
+   * @return {@link Rotation3d} from the IMU.
    */
   @Override
-  public void getYawPitchRoll(double[] yprArray)
+  public Rotation3d getRawRotation3d()
   {
-    imu.getYawPitchRoll(yprArray);
+    double[] wxyz = new double[4];
+    imu.get6dQuaternion(wxyz);
+    return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
   }
 
   /**
@@ -77,9 +83,7 @@ public class PigeonSwerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    double[] wxyz = new double[4];
-    imu.get6dQuaternion(wxyz);
-    return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
+    return getRawRotation3d().minus(offset);
   }
 
   /**