From 64566c4d711a0bffc2136056aa5533cd757ade31 Mon Sep 17 00:00:00 2001
From: Curt <clrozeboom@gmail.com>
Date: Sun, 28 Jul 2024 18:27:33 -0400
Subject: [PATCH 1/3] Add ability to get rotation rate from the IMU

---
 src/main/java/swervelib/imu/ADIS16448Swerve.java  | 8 ++++++++
 src/main/java/swervelib/imu/ADIS16470Swerve.java  | 8 ++++++++
 src/main/java/swervelib/imu/ADXRS450Swerve.java   | 8 ++++++++
 src/main/java/swervelib/imu/AnalogGyroSwerve.java | 8 ++++++++
 src/main/java/swervelib/imu/NavXSwerve.java       | 8 ++++++++
 src/main/java/swervelib/imu/Pigeon2Swerve.java    | 8 ++++++++
 src/main/java/swervelib/imu/PigeonSwerve.java     | 8 ++++++++
 src/main/java/swervelib/imu/SwerveIMU.java        | 6 ++++++
 8 files changed, 62 insertions(+)

diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java
index 4295d483..1663e222 100644
--- a/src/main/java/swervelib/imu/ADIS16448Swerve.java
+++ b/src/main/java/swervelib/imu/ADIS16448Swerve.java
@@ -110,6 +110,14 @@ public Optional<Translation3d> getAccel()
     return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
   }
 
+  /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public Optional<Double> getRate() {
+    return Optional.of(imu.getRate());
+  }
+
   /**
    * Get the instantiated IMU object.
    *
diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java
index 94a31b76..7b0647a0 100644
--- a/src/main/java/swervelib/imu/ADIS16470Swerve.java
+++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java
@@ -110,6 +110,14 @@ public Optional<Translation3d> getAccel()
     return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
   }
 
+  /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public Optional<Double> getRate() {
+    return Optional.of(imu.getRate());
+  }
+
   /**
    * Get the instantiated IMU object.
    *
diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java
index 718a8966..b283da3c 100644
--- a/src/main/java/swervelib/imu/ADXRS450Swerve.java
+++ b/src/main/java/swervelib/imu/ADXRS450Swerve.java
@@ -108,6 +108,14 @@ public Optional<Translation3d> getAccel()
     return Optional.empty();
   }
 
+  /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public Optional<Double> getRate() {
+    return Optional.of(imu.getRate());
+  }
+
   /**
    * Get the instantiated IMU object.
    *
diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java
index d104a0ff..51bc23b0 100644
--- a/src/main/java/swervelib/imu/AnalogGyroSwerve.java
+++ b/src/main/java/swervelib/imu/AnalogGyroSwerve.java
@@ -115,6 +115,14 @@ public Optional<Translation3d> getAccel()
     return Optional.empty();
   }
 
+  /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public Optional<Double> getRate() {
+    return Optional.of(gyro.getRate());
+  }
+
   /**
    * Get the instantiated IMU object.
    *
diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java
index 8b2b239f..ddddc7ac 100644
--- a/src/main/java/swervelib/imu/NavXSwerve.java
+++ b/src/main/java/swervelib/imu/NavXSwerve.java
@@ -177,6 +177,14 @@ public Optional<Translation3d> getAccel()
             .times(9.81));
   }
 
+  /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public Optional<Double> getRate() {
+    return Optional.of(gyro.getRate());
+  }
+
   /**
    * Get the instantiated IMU object.
    *
diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java
index bc89264c..dcc5924c 100644
--- a/src/main/java/swervelib/imu/Pigeon2Swerve.java
+++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java
@@ -138,6 +138,14 @@ public Optional<Translation3d> getAccel()
                                          zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()).times(9.81 / 16384.0));
   }
 
+  /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public Optional<Double> getRate() {
+    return Optional.of(imu.getRate());
+  }
+
   /**
    * Get the instantiated IMU object.
    *
diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java
index b9bbcc5c..5575ccbb 100644
--- a/src/main/java/swervelib/imu/PigeonSwerve.java
+++ b/src/main/java/swervelib/imu/PigeonSwerve.java
@@ -115,6 +115,14 @@ public Optional<Translation3d> getAccel()
     return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
   }
 
+   /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public Optional<Double> getRate() {
+    return Optional.of(imu.getRate());
+  }
+
   /**
    * Get the instantiated IMU object.
    *
diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java
index dd9a727a..4d6bed2c 100644
--- a/src/main/java/swervelib/imu/SwerveIMU.java
+++ b/src/main/java/swervelib/imu/SwerveIMU.java
@@ -56,6 +56,12 @@ public abstract class SwerveIMU
    */
   public abstract Optional<Translation3d> getAccel();
 
+  /**
+   * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
+   * @return {@link Double} of the rotation rate as an {@link Optional}.
+   */
+  public abstract Optional<Double> getRate();
+
   /**
    * Get the instantiated IMU object.
    *

From 6b6d3c036f382b33a67689311ab04d94d4065798 Mon Sep 17 00:00:00 2001
From: Curt <clrozeboom@gmail.com>
Date: Tue, 30 Jul 2024 17:41:59 -0400
Subject: [PATCH 2/3] Just use double not Optional and use imu consistently

---
 .../java/swervelib/imu/ADIS16448Swerve.java   |  4 +--
 .../java/swervelib/imu/ADIS16470Swerve.java   |  4 +--
 .../java/swervelib/imu/ADXRS450Swerve.java    |  4 +--
 .../java/swervelib/imu/AnalogGyroSwerve.java  | 16 +++++-----
 src/main/java/swervelib/imu/NavXSwerve.java   | 30 +++++++++----------
 src/main/java/swervelib/imu/PigeonSwerve.java |  4 +--
 src/main/java/swervelib/imu/SwerveIMU.java    |  2 +-
 7 files changed, 32 insertions(+), 32 deletions(-)

diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java
index 1663e222..531b30d7 100644
--- a/src/main/java/swervelib/imu/ADIS16448Swerve.java
+++ b/src/main/java/swervelib/imu/ADIS16448Swerve.java
@@ -114,8 +114,8 @@ public Optional<Translation3d> getAccel()
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public Optional<Double> getRate() {
-    return Optional.of(imu.getRate());
+  public double getRate() {
+    return imu.getRate();
   }
 
   /**
diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java
index 7b0647a0..fd01a3d0 100644
--- a/src/main/java/swervelib/imu/ADIS16470Swerve.java
+++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java
@@ -114,8 +114,8 @@ public Optional<Translation3d> getAccel()
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public Optional<Double> getRate() {
-    return Optional.of(imu.getRate());
+  public double getRate() {
+    return imu.getRate();
   }
 
   /**
diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java
index b283da3c..dd9a72d9 100644
--- a/src/main/java/swervelib/imu/ADXRS450Swerve.java
+++ b/src/main/java/swervelib/imu/ADXRS450Swerve.java
@@ -112,8 +112,8 @@ public Optional<Translation3d> getAccel()
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public Optional<Double> getRate() {
-    return Optional.of(imu.getRate());
+  public double getRate() {
+    return imu.getRate();
   }
 
   /**
diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java
index 51bc23b0..e789d97e 100644
--- a/src/main/java/swervelib/imu/AnalogGyroSwerve.java
+++ b/src/main/java/swervelib/imu/AnalogGyroSwerve.java
@@ -15,7 +15,7 @@ public class AnalogGyroSwerve extends SwerveIMU
   /**
    * Gyroscope object.
    */
-  private final AnalogGyro gyro;
+  private final AnalogGyro imu;
   /**
    * Offset for the analog gyro.
    */
@@ -37,9 +37,9 @@ public AnalogGyroSwerve(int channel)
       throw new RuntimeException(
           "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n");
     }
-    gyro = new AnalogGyro(channel);
+    imu = new AnalogGyro(channel);
     factoryDefault();
-    SmartDashboard.putData(gyro);
+    SmartDashboard.putData(imu);
   }
 
   /**
@@ -48,7 +48,7 @@ public AnalogGyroSwerve(int channel)
   @Override
   public void factoryDefault()
   {
-    gyro.calibrate();
+    imu.calibrate();
     offset = new Rotation3d(0, 0, 0);
   }
 
@@ -88,7 +88,7 @@ public void setInverted(boolean invertIMU)
    */
   public Rotation3d getRawRotation3d()
   {
-    Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle()));
+    Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle()));
     return invertedIMU ? reading.unaryMinus() : reading;
   }
 
@@ -119,8 +119,8 @@ public Optional<Translation3d> getAccel()
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public Optional<Double> getRate() {
-    return Optional.of(gyro.getRate());
+  public double getRate() {
+    return imu.getRate();
   }
 
   /**
@@ -131,6 +131,6 @@ public Optional<Double> getRate() {
   @Override
   public Object getIMU()
   {
-    return gyro;
+    return imu;
   }
 }
diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java
index ddddc7ac..05c50a30 100644
--- a/src/main/java/swervelib/imu/NavXSwerve.java
+++ b/src/main/java/swervelib/imu/NavXSwerve.java
@@ -19,7 +19,7 @@ public class NavXSwerve extends SwerveIMU
   /**
    * NavX IMU.
    */
-  private AHRS       gyro;
+  private AHRS       imu;
   /**
    * Offset for the NavX.
    */
@@ -46,9 +46,9 @@ public NavXSwerve(SerialPort.Port port)
       /* Communicate w/navX-MXP via the MXP SPI Bus.                                     */
       /* Alternatively:  I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB     */
       /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
-      gyro = new AHRS(port);
+      imu = new AHRS(port);
       factoryDefault();
-      SmartDashboard.putData(gyro);
+      SmartDashboard.putData(imu);
     } catch (RuntimeException ex)
     {
       navXError.setText("Error instantiating NavX: " + ex.getMessage());
@@ -68,9 +68,9 @@ public NavXSwerve(SPI.Port port)
       /* Communicate w/navX-MXP via the MXP SPI Bus.                                     */
       /* Alternatively:  I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB     */
       /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
-      gyro = new AHRS(port);
+      imu = new AHRS(port);
       factoryDefault();
-      SmartDashboard.putData(gyro);
+      SmartDashboard.putData(imu);
     } catch (RuntimeException ex)
     {
       navXError.setText("Error instantiating NavX: " + ex.getMessage());
@@ -90,9 +90,9 @@ public NavXSwerve(I2C.Port port)
       /* Communicate w/navX-MXP via the MXP SPI Bus.                                     */
       /* Alternatively:  I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB     */
       /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
-      gyro = new AHRS(port);
+      imu = new AHRS(port);
       factoryDefault();
-      SmartDashboard.putData(gyro);
+      SmartDashboard.putData(imu);
     } catch (RuntimeException ex)
     {
       navXError.setText("Error instantiating NavX: " + ex.getMessage());
@@ -107,7 +107,7 @@ public NavXSwerve(I2C.Port port)
   public void factoryDefault()
   {
     // gyro.reset(); // Reported to be slow
-    offset = gyro.getRotation3d();
+    offset = imu.getRotation3d();
   }
 
   /**
@@ -146,7 +146,7 @@ public void setInverted(boolean invertIMU)
   @Override
   public Rotation3d getRawRotation3d()
   {
-    return invertedIMU ? gyro.getRotation3d().unaryMinus() : gyro.getRotation3d();
+    return invertedIMU ? imu.getRotation3d().unaryMinus() : imu.getRotation3d();
   }
 
   /**
@@ -171,9 +171,9 @@ public Optional<Translation3d> getAccel()
   {
     return Optional.of(
         new Translation3d(
-            gyro.getWorldLinearAccelX(),
-            gyro.getWorldLinearAccelY(),
-            gyro.getWorldLinearAccelZ())
+            imu.getWorldLinearAccelX(),
+            imu.getWorldLinearAccelY(),
+            imu.getWorldLinearAccelZ())
             .times(9.81));
   }
 
@@ -181,8 +181,8 @@ public Optional<Translation3d> getAccel()
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public Optional<Double> getRate() {
-    return Optional.of(gyro.getRate());
+  public double getRate() {
+    return imu.getRate();
   }
 
   /**
@@ -193,6 +193,6 @@ public Optional<Double> getRate() {
   @Override
   public Object getIMU()
   {
-    return gyro;
+    return imu;
   }
 }
diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java
index 5575ccbb..25e12fbc 100644
--- a/src/main/java/swervelib/imu/PigeonSwerve.java
+++ b/src/main/java/swervelib/imu/PigeonSwerve.java
@@ -119,8 +119,8 @@ public Optional<Translation3d> getAccel()
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public Optional<Double> getRate() {
-    return Optional.of(imu.getRate());
+  public double getRate() {
+    return imu.getRate();
   }
 
   /**
diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java
index 4d6bed2c..ca2b2344 100644
--- a/src/main/java/swervelib/imu/SwerveIMU.java
+++ b/src/main/java/swervelib/imu/SwerveIMU.java
@@ -60,7 +60,7 @@ public abstract class SwerveIMU
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public abstract Optional<Double> getRate();
+  public abstract double getRate();
 
   /**
    * Get the instantiated IMU object.

From 51e87eedbb6e73e91ee1bc48cc99bb82b253d225 Mon Sep 17 00:00:00 2001
From: Curt <clrozeboom@gmail.com>
Date: Tue, 30 Jul 2024 18:08:10 -0400
Subject: [PATCH 3/3] Missed Pigeon2

---
 src/main/java/swervelib/imu/Pigeon2Swerve.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java
index dcc5924c..6498dba3 100644
--- a/src/main/java/swervelib/imu/Pigeon2Swerve.java
+++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java
@@ -142,8 +142,8 @@ public Optional<Translation3d> getAccel()
    * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
    * @return {@link Double} of the rotation rate as an {@link Optional}.
    */
-  public Optional<Double> getRate() {
-    return Optional.of(imu.getRate());
+  public double getRate() {
+    return imu.getRate();
   }
 
   /**