Merge pull request #140 from Technologyman00/main

Functionalize IMU Inversions
BroncBotz3481 · Jan 22, 2024 · d1b07d2 · d1b07d2
2 parents 7fe6baa + ca3c2a0
commit d1b07d2
Show file tree

Hide file tree

Showing 10 changed files with 139 additions and 20 deletions.
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
@@ -729,9 +729,7 @@ public Rotation2d getYaw()
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
     if (!SwerveDriveTelemetry.isSimulation)
     {
-      return swerveDriveConfiguration.invertedIMU
-             ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ())
-             : Rotation2d.fromRadians(imu.getRotation3d().getZ());
+      return Rotation2d.fromRadians(imu.getRotation3d().getZ());
     } else
     {
       return simIMU.getYaw();
@@ -748,9 +746,7 @@ public Rotation2d getPitch()
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
     if (!SwerveDriveTelemetry.isSimulation)
     {
-      return swerveDriveConfiguration.invertedIMU
-             ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY())
-             : Rotation2d.fromRadians(imu.getRotation3d().getY());
+      return Rotation2d.fromRadians(imu.getRotation3d().getY());
     } else
     {
       return simIMU.getPitch();
@@ -767,9 +763,7 @@ public Rotation2d getRoll()
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
     if (!SwerveDriveTelemetry.isSimulation)
     {
-      return swerveDriveConfiguration.invertedIMU
-             ? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX())
-             : Rotation2d.fromRadians(imu.getRotation3d().getX());
+      return Rotation2d.fromRadians(imu.getRotation3d().getX());
     } else
     {
       return simIMU.getRoll();
@@ -786,9 +780,7 @@ public Rotation3d getGyroRotation3d()
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
     if (!SwerveDriveTelemetry.isSimulation)
     {
-      return swerveDriveConfiguration.invertedIMU
-             ? imu.getRotation3d().unaryMinus()
-             : imu.getRotation3d();
+      return imu.getRotation3d();
     } else
     {
       return simIMU.getGyroRotation3d();

diff --git a/src/main/java/swervelib/imu/ADIS16448Swerve.java b/src/main/java/swervelib/imu/ADIS16448Swerve.java
@@ -20,6 +20,10 @@ public class ADIS16448Swerve extends SwerveIMU
    * Offset for the ADIS16448.
    */
   private       Rotation3d    offset = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private boolean invertedIMU = false;
 
   /**
    * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -60,13 +64,28 @@ public void setOffset(Rotation3d offset)
     this.offset = offset;
   }
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
    * @return {@link Rotation3d} from the IMU.
    */
   public Rotation3d getRawRotation3d()
   {
+    if(invertedIMU){
+      return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()),
+                            Math.toRadians(-imu.getGyroAngleY()),
+                            Math.toRadians(-imu.getGyroAngleZ())).unaryMinus();
+    }
     return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()),
                           Math.toRadians(-imu.getGyroAngleY()),
                           Math.toRadians(-imu.getGyroAngleZ()));

diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java
@@ -21,6 +21,10 @@ public class ADIS16470Swerve extends SwerveIMU
    * Offset for the ADIS16470.
    */
   private       Rotation3d    offset = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private boolean invertedIMU = false;
 
   /**
    * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -62,13 +66,26 @@ public void setOffset(Rotation3d offset)
     this.offset = offset;
   }
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
    * @return {@link Rotation3d} from the IMU.
    */
   public Rotation3d getRawRotation3d()
   {
+    if(invertedIMU){
+      return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw))).unaryMinus();
+    }
     return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw)));
   }
 

diff --git a/src/main/java/swervelib/imu/ADXRS450Swerve.java b/src/main/java/swervelib/imu/ADXRS450Swerve.java
@@ -20,6 +20,10 @@ public class ADXRS450Swerve extends SwerveIMU
    * Offset for the ADXRS450.
    */
   private       Rotation3d    offset = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private boolean invertedIMU = false;
 
   /**
    * Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -60,13 +64,26 @@ public void setOffset(Rotation3d offset)
     this.offset = offset;
   }
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
    * @return {@link Rotation3d} from the IMU.
    */
   public Rotation3d getRawRotation3d()
   {
+    if(invertedIMU){
+      return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle())).unaryMinus();
+    }
     return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle()));
   }
 

diff --git a/src/main/java/swervelib/imu/AnalogGyroSwerve.java b/src/main/java/swervelib/imu/AnalogGyroSwerve.java
@@ -20,6 +20,10 @@ public class AnalogGyroSwerve extends SwerveIMU
    * Offset for the analog gyro.
    */
   private       Rotation3d offset = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private boolean invertedIMU = false;
 
   /**
    * Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1.
@@ -67,13 +71,26 @@ public void setOffset(Rotation3d offset)
     this.offset = offset;
   }
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
    * @return {@link Rotation3d} from the IMU.
    */
   public Rotation3d getRawRotation3d()
   {
+    if(invertedIMU){
+      return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle())).unaryMinus();
+    }
     return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle()));
   }
 

diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java
@@ -24,6 +24,10 @@ public class NavXSwerve extends SwerveIMU
    * Offset for the NavX.
    */
   private Rotation3d offset = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private boolean invertedIMU = false;
   /**
    * An {@link Alert} for if there is an error instantiating the NavX.
    */
@@ -124,6 +128,16 @@ public void setOffset(Rotation3d offset)
     this.offset = offset;
   }
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
@@ -132,6 +146,9 @@ public void setOffset(Rotation3d offset)
   @Override
   public Rotation3d getRawRotation3d()
   {
+    if(invertedIMU){
+      return gyro.getRotation3d().unaryMinus();
+    }
     return gyro.getRotation3d();
   }
 

diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java
@@ -24,6 +24,10 @@ public class Pigeon2Swerve extends SwerveIMU
    * Offset for the Pigeon 2.
    */
   private Rotation3d offset = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private boolean invertedIMU = false;
 
   /**
    * Generate the SwerveIMU for pigeon.
@@ -79,6 +83,16 @@ public void setOffset(Rotation3d offset)
     this.offset = offset;
   }
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
@@ -88,14 +102,15 @@ public void setOffset(Rotation3d offset)
   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();
-    return new Rotation3d(new Quaternion(w.refresh().getValue(),
-                                         x.refresh().getValue(),
-                                         y.refresh().getValue(),
-                                         z.refresh().getValue()));
+    StatusSignal<Double> w    = imu.getQuatW();
+    StatusSignal<Double> x    = imu.getQuatX();
+    StatusSignal<Double> y    = imu.getQuatY();
+    StatusSignal<Double> z    = imu.getQuatZ();
+
+    if(invertedIMU){
+      return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue())).unaryMinus();
+    }
+    return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue()));
   }
 
   /**

diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java
@@ -21,6 +21,10 @@ public class PigeonSwerve extends SwerveIMU
    * Offset for the Pigeon.
    */
   private Rotation3d offset = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private boolean invertedIMU = false;
 
   /**
    * Generate the SwerveIMU for pigeon.
@@ -62,6 +66,16 @@ public void setOffset(Rotation3d offset)
     this.offset = offset;
   }
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *
@@ -72,6 +86,9 @@ public Rotation3d getRawRotation3d()
   {
     double[] wxyz = new double[4];
     imu.get6dQuaternion(wxyz);
+    if(invertedIMU){
+      return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])).unaryMinus();
+    }
     return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
   }
 

diff --git a/src/main/java/swervelib/imu/SwerveIMU.java b/src/main/java/swervelib/imu/SwerveIMU.java
@@ -27,6 +27,13 @@ public abstract class SwerveIMU
    */
   public abstract void setOffset(Rotation3d offset);
 
+  /**
+   * Set the gyro to invert its default direction
+   * 
+   * @param invert gyro direction
+   */
+  public abstract void setInverted(boolean invertIMU);
+
   /**
    * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
    *

diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java
@@ -55,6 +55,7 @@ public SwerveDriveConfiguration(
     this.moduleCount = moduleConfigs.length;
     this.imu = swerveIMU;
     this.invertedIMU = invertedIMU;
+    swerveIMU.setInverted(invertedIMU);
     this.modules = createModules(moduleConfigs, driveFeedforward);
     this.moduleLocationsMeters = new Translation2d[moduleConfigs.length];
     for (SwerveModule module : modules)