Merge pull request #216 from FRC5010/dev

Add ability to get the rotation rate from the IMU
BroncBotz3481 · Jul 30, 2024 · 89f1801 · 89f1801
2 parents 7451ce3 + 51e87ee
commit 89f1801
Show file tree

Hide file tree

Showing 8 changed files with 81 additions and 19 deletions.
diff --git 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 double getRate() {
+    return 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
@@ -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 double getRate() {
+    return 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
@@ -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 double getRate() {
+    return 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
@@ -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;
   }
 
@@ -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 double getRate() {
+    return imu.getRate();
+  }
+
   /**
    * Get the instantiated IMU object.
    *
@@ -123,6 +131,6 @@ public Optional<Translation3d> getAccel()
   @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
@@ -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,12 +171,20 @@ public Optional<Translation3d> getAccel()
   {
     return Optional.of(
         new Translation3d(
-            gyro.getWorldLinearAccelX(),
-            gyro.getWorldLinearAccelY(),
-            gyro.getWorldLinearAccelZ())
+            imu.getWorldLinearAccelX(),
+            imu.getWorldLinearAccelY(),
+            imu.getWorldLinearAccelZ())
             .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 double getRate() {
+    return imu.getRate();
+  }
+
   /**
    * Get the instantiated IMU object.
    *
@@ -185,6 +193,6 @@ public Optional<Translation3d> getAccel()
   @Override
   public Object getIMU()
   {
-    return gyro;
+    return imu;
   }
 }
diff --git 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 double getRate() {
+    return 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
@@ -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 double getRate() {
+    return 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
@@ -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 double getRate();
+
   /**
    * Get the instantiated IMU object.
    *