Improved inverted IMU's using ternary operations. Removed debug parameter. Removed redundant inversion from SwerveDriveConfiguration. Added getOdometryHeading. Utilize getOdometryHeading alot within SwerveDrive. Fixed SwerveSubsystem. Fixed vision measurement issue. Removed dedicated visionStdDevs and stateStdDevs.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

Author: thenetworkgrinch

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 java {

simgui.json

@@ -85,6 +85,12 @@
           "arrowWeight": 3.0,
           "style": "Hidden"
         },
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
         "window": {
           "visible": true
         }

src/main/java/swervelib/SwerveDrive.java

@@ -1,7 +1,6 @@
 package swervelib;
 
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.filter.SlewRateLimiter;
@@ -72,9 +71,12 @@ public class SwerveDrive
    */
   private final Lock                     odometryLock                                    = new ReentrantLock();
   /**
-   * Deadband for speeds in heading correction.
+   * Alert to recommend Tuner X if the configuration is compatible.
    */
-  private       double                   HEADING_CORRECTION_DEADBAND                     = 0.01;
+  private final Alert                    tunerXRecommendation                            = new Alert("Swerve Drive",
+                                                                                                     "Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
+                                                                                                     "https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
+                                                                                                     AlertType.WARNING);
   /**
    * Field object.
    */
@@ -83,26 +85,6 @@ public class SwerveDrive
    * Swerve controller for controlling heading of the robot.
    */
   public        SwerveController         swerveController;
-  /**
-   * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of
-   * rotation)
-   */
-  public        Matrix<N3, N1>           stateStdDevs                                    = VecBuilder.fill(0.1,
-                                                                                                           0.1,
-                                                                                                           0.1);
-  /**
-   * The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more
-   * points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)}
-   * with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get
-   * vision accurate to inches instead of feet.
-   */
-  public        Matrix<N3, N1>           visionMeasurementStdDevs                        = VecBuilder.fill(0.9,
-                                                                                                           0.9,
-                                                                                                           0.9);
-  /**
-   * Invert odometry readings of drive motor positions, used as a patch for debugging currently.
-   */
-  public        boolean                  invertOdometry                                  = false;
   /**
    * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
    * correction.
@@ -112,6 +94,10 @@ public class SwerveDrive
    * Whether to correct heading when driving translationally. Set to true to enable.
    */
   public        boolean                  headingCorrection                               = false;
+  /**
+   * Deadband for speeds in heading correction.
+   */
+  private       double                   HEADING_CORRECTION_DEADBAND                     = 0.01;
   /**
    * Whether heading correction PID is currently active.
    */
@@ -144,13 +130,6 @@ public class SwerveDrive
    * Maximum speed of the robot in meters per second.
    */
   private       double                   maxSpeedMPS;
-  /**
-   * Alert to recommend Tuner X if the configuration is compatible.
-   */
-  private final Alert                    tunerXRecommendation                            = new Alert("Swerve Drive",
-                                                                                                     "Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
-                                                                                                     "https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
-                                                                                                     AlertType.WARNING);
 
   /**
    * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -194,9 +173,8 @@ public SwerveDrive(
             kinematics,
             getYaw(),
             getModulePositions(),
-            new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
-            stateStdDevs,
-            visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, higher=less weight
+            new Pose2d(new Translation2d(0, 0),
+                       Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight
 
     zeroGyro();
 
@@ -308,6 +286,16 @@ public void setDriveMotorConversionFactor(double conversionFactor)
     }
   }
 
+  /**
+   * Fetch the latest odometry heading, should be trusted over {@link SwerveDrive#getYaw()}.
+   *
+   * @return {@link Rotation2d} of the robot heading.
+   */
+  public Rotation2d getOdometryHeading()
+  {
+    return swerveDrivePoseEstimator.getEstimatedPosition().getRotation();
+  }
+
   /**
    * Set the heading correction capabilities of YAGSL.
    *
@@ -337,7 +325,7 @@ public void setHeadingCorrection(boolean state, double deadband)
    */
   public void driveFieldOriented(ChassisSpeeds velocity)
   {
-    ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw());
+    ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
     drive(fieldOrientedVelocity);
   }
 
@@ -349,7 +337,7 @@ public void driveFieldOriented(ChassisSpeeds velocity)
    */
   public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
   {
-    ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw());
+    ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
     drive(fieldOrientedVelocity, centerOfRotationMeters);
   }
 
@@ -401,7 +389,7 @@ public void drive(
     ChassisSpeeds velocity =
         fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            translation.getX(), translation.getY(), rotation, getYaw())
+            translation.getX(), translation.getY(), rotation, getOdometryHeading())
         : new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
 
     drive(velocity, isOpenLoop, centerOfRotationMeters);
@@ -430,7 +418,7 @@ public void drive(
     ChassisSpeeds velocity =
         fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            translation.getX(), translation.getY(), rotation, getYaw())
+            translation.getX(), translation.getY(), rotation, getOdometryHeading())
         : new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
 
     drive(velocity, isOpenLoop, new Translation2d());
@@ -466,11 +454,11 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
       {
         if (!correctionEnabled)
         {
-          lastHeadingRadians = getYaw().getRadians();
+          lastHeadingRadians = getOdometryHeading().getRadians();
           correctionEnabled = true;
         }
         velocity.omegaRadiansPerSecond =
-            swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
+            swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians());
       } else
       {
         correctionEnabled = false;
@@ -623,7 +611,7 @@ public ChassisSpeeds getFieldVelocity()
     // angle
     // given as the robot angle reverses the direction of rotation, and the conversion is reversed.
     return ChassisSpeeds.fromFieldRelativeSpeeds(
-        kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
+        kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
   }
 
   /**
@@ -680,8 +668,7 @@ public SwerveModuleState[] getStates()
   }
 
   /**
-   * Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance from each module if
-   * {@link #invertOdometry} is true.
+   * Gets the current module positions (azimuth and wheel position (meters)).
    *
    * @return A list of SwerveModulePositions containg the current module positions
    */
@@ -692,10 +679,6 @@ public SwerveModulePosition[] getModulePositions()
     for (SwerveModule module : swerveModules)
     {
       positions[module.moduleNumber] = module.getPosition();
-      if (invertOdometry)
-      {
-        positions[module.moduleNumber].distanceMeters *= -1;
-      }
     }
     return positions;
   }
@@ -945,7 +928,7 @@ public void updateOdometry()
         SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
         SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
         SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
-        SwerveDriveTelemetry.robotRotation = getYaw().getDegrees();
+        SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees();
       }
 
       if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
@@ -961,7 +944,8 @@ public void updateOdometry()
         if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
         {
           module.updateTelemetry();
-          SmartDashboard.putNumber("Adjusted IMU Yaw", getYaw().getDegrees());
+          SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees());
+          SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees());
         }
         if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
         {
@@ -1019,28 +1003,6 @@ public void setGyroOffset(Rotation3d offset)
     }
   }
 
-  /**
-   * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
-   * the given timestamp of the vision measurement. <br /> <b>IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
-   * AFTER USING THIS FUNCTION!</b> <br /> To update your gyroscope readings directly use
-   * {@link SwerveDrive#setGyroOffset(Rotation3d)}.
-   *
-   * @param robotPose Robot {@link Pose2d} as measured by vision.
-   * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
-   *                  {@link Timer#getFPGATimestamp()} or similar sources.
-   */
-  public void addVisionMeasurement(Pose2d robotPose, double timestamp)
-  {
-    odometryLock.lock();
-    swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
-    Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(),
-                                    robotPose.getRotation());
-    odometryLock.unlock();
-
-    setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
-    resetOdometry(newOdometry);
-  }
-
   /**
    * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
    * the given timestamp of the vision measurement.
@@ -1058,8 +1020,31 @@ public void addVisionMeasurement(Pose2d robotPose, double timestamp)
   public void addVisionMeasurement(Pose2d robotPose, double timestamp,
                                    Matrix<N3, N1> visionMeasurementStdDevs)
   {
-    this.visionMeasurementStdDevs = visionMeasurementStdDevs;
-    addVisionMeasurement(robotPose, timestamp);
+    odometryLock.lock();
+    swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs);
+    odometryLock.unlock();
+  }
+
+  /**
+   * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
+   * the given timestamp of the vision measurement. <br /> <b>IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
+   * AFTER USING THIS FUNCTION!</b> <br /> To update your gyroscope readings directly use
+   * {@link SwerveDrive#setGyroOffset(Rotation3d)}.
+   *
+   * @param robotPose Robot {@link Pose2d} as measured by vision.
+   * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
+   *                  {@link Timer#getFPGATimestamp()} or similar sources.
+   */
+  public void addVisionMeasurement(Pose2d robotPose, double timestamp)
+  {
+    odometryLock.lock();
+    swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
+//    Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(),
+//                                    robotPose.getRotation());
+    odometryLock.unlock();
+
+//    setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
+//    resetOdometry(newOdometry);
   }
 
 

src/main/java/swervelib/imu/ADIS16448Swerve.java

@@ -19,11 +19,11 @@ public class ADIS16448Swerve extends SwerveIMU
   /**
    * Offset for the ADIS16448.
    */
-  private       Rotation3d    offset = new Rotation3d();
+  private       Rotation3d    offset      = new Rotation3d();
   /**
    * Inversion for the gyro
    */
-  private boolean invertedIMU = false;
+  private       boolean       invertedIMU = false;
 
   /**
    * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -66,7 +66,7 @@ public void setOffset(Rotation3d offset)
 
   /**
    * Set the gyro to invert its default direction
-   * 
+   *
    * @param invertIMU invert gyro direction
    */
   public void setInverted(boolean invertIMU)
@@ -81,14 +81,10 @@ public void setInverted(boolean invertIMU)
    */
   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()));
+    Rotation3d reading = new Rotation3d(Math.toRadians(-imu.getGyroAngleX()),
+                                        Math.toRadians(-imu.getGyroAngleY()),
+                                        Math.toRadians(-imu.getGyroAngleZ()));
+    return invertedIMU ? reading.unaryMinus() : reading;
   }
 
   /**

src/main/java/swervelib/imu/ADIS16470Swerve.java

@@ -20,11 +20,11 @@ public class ADIS16470Swerve extends SwerveIMU
   /**
    * Offset for the ADIS16470.
    */
-  private       Rotation3d    offset = new Rotation3d();
+  private       Rotation3d    offset      = new Rotation3d();
   /**
    * Inversion for the gyro
    */
-  private boolean invertedIMU = false;
+  private       boolean       invertedIMU = false;
 
   /**
    * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -68,7 +68,7 @@ public void setOffset(Rotation3d offset)
 
   /**
    * Set the gyro to invert its default direction
-   * 
+   *
    * @param invertIMU invert gyro direction
    */
   public void setInverted(boolean invertIMU)
@@ -83,10 +83,8 @@ public void setInverted(boolean invertIMU)
    */
   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)));
+    Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw)));
+    return invertedIMU ? reading.unaryMinus() : reading;
   }
 
   /**