From a1521ee45b7ee076108afa1b2489718224f2d73a Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 14:50:48 -0400
Subject: [PATCH 01/18] fixed spelling and changed getFPGATime()

---
 src/main/java/swervelib/SwerveDrive.java     | 10 +++++-----
 src/main/java/swervelib/imu/IMUVelocity.java |  6 +++---
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index 4fe49f5f..9ebf1316 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -498,7 +498,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
     if(angularVelocityCorrection)
     {
       // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now
-      // This multipler should be built into the imuVelocity.getVelocity() function and is almost certainly dependand on the timestep used
+      // This multipler should be built into the imuVelocity.getVelocity() method and is almost certainly dependant on the timestep used
       var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * 1000000 * angularVelocityCoefficient);
       if(angularVelocity.getRadians() != 0.0){
         velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
@@ -650,9 +650,9 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
     /**
    * Set chassis speeds with closed-loop velocity control, using the optimizations present 
    * in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. This may improve
-   * performance during autonomous, if the optimizations make you drive straight in teleop then 
-   * they should help in autonomous, think of this as a feedforward that reduces the correction needed
-   * from PID due to skew.
+   * performance during autonomous, if the optimizations reduce your bot's skew in teleop then 
+   * they should help in autonomous, think of this as a feedforward that reduces the correction 
+   * needed from PID due to skew.
    *
    * @param chassisSpeeds Chassis speeds to set.
    */
@@ -662,7 +662,7 @@ public void setChassisSpeedsWithDriveOptimizations(ChassisSpeeds chassisSpeeds)
     if(angularVelocityCorrection)
     {
       // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now
-      // This multipler should be built into the imuVelocity.getVelocity() function and is almost certainly dependand on the timestep used
+      // This multipler should be built into the imuVelocity.getVelocity() function and is almost certainly dependant on the timestep used
       var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * 1000000 * angularVelocityCoefficient);
       if(angularVelocity.getRadians() != 0.0){
         chassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(
diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java
index 3a1df320..45ad8e9e 100644
--- a/src/main/java/swervelib/imu/IMUVelocity.java
+++ b/src/main/java/swervelib/imu/IMUVelocity.java
@@ -1,9 +1,9 @@
 package swervelib.imu;
 
+import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.Notifier;
-import edu.wpi.first.wpilibj.RobotController;
 
 public class IMUVelocity {
   /**
@@ -66,7 +66,7 @@ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
     velocityFilter = LinearFilter.movingAverage(averagingTaps);
     notifier = new Notifier(this::update);
     notifier.startPeriodic(periodSeconds);
-    timestamp = RobotController.getFPGATime();
+    timestamp = HALUtil.getFPGATime();
   }
 
   /**
@@ -74,7 +74,7 @@ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
    */
   private void update() 
   {
-    double newTimestamp = RobotController.getFPGATime();
+    double newTimestamp = HALUtil.getFPGATime();
     Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ());
 
     synchronized (this) {

From 45d4f9ed4b979a0fee120cc93e163c51b72e9bd9 Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 18:49:36 -0400
Subject: [PATCH 02/18] found gyro rates and made requested changes

---
 src/main/java/swervelib/SwerveDrive.java     | 37 +++++++----
 src/main/java/swervelib/imu/IMUVelocity.java | 69 ++++++++++++++++----
 2 files changed, 79 insertions(+), 27 deletions(-)

diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index 9ebf1316..130f99f9 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -189,6 +189,11 @@ public SwerveDrive(
       imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
     }
 
+    if(angularVelocityCorrection)
+    {
+      setupIMUVelocity();
+    }
+
     this.swerveModules = config.modules;
 
     //    odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
@@ -493,8 +498,6 @@ public void drive(
    */
   public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
   {
-
-    // Explanation coming soon
     if(angularVelocityCorrection)
     {
       // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now
@@ -658,7 +661,6 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
    */
   public void setChassisSpeedsWithDriveOptimizations(ChassisSpeeds chassisSpeeds)
   {
-    // Explanation coming soon
     if(angularVelocityCorrection)
     {
       // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now
@@ -1284,8 +1286,12 @@ public void setCosineCompensator(boolean enabled)
    */
   public void setChassisDiscretization(boolean enable, double dtSeconds)
   {
-    chassisVelocityCorrection = enable;
-    discretizationdtSeconds = dtSeconds;
+    if(!SwerveDriveTelemetry.isSimulation)
+    {
+      chassisVelocityCorrection = enable;
+      discretizationdtSeconds = dtSeconds;
+    }
+
   }
 
   /**
@@ -1300,15 +1306,18 @@ public void setChassisDiscretization(boolean enable, double dtSeconds)
    */
   public void setAngularVelocityCompensation(boolean enable, double angularVelocityCoeff)
   {
-    /** TODO: How should it behave in the case of simulation? Skew doesn't really exist in simulation, 
-     * discretize could also be removed in simulation as it creates skew in the simulation, 
-     * the skew it corrects is only present on a real bot.
-     * This makes the robot continously rotate due to a lack of friction.
-     * My vote is to disable both discretize and angular velocity correction.
-     */
-    imuVelocity = new IMUVelocity(imu);
-    angularVelocityCorrection = true;
-    angularVelocityCoefficient = angularVelocityCoeff;
+    if(!SwerveDriveTelemetry.isSimulation)
+    {
+      angularVelocityCorrection = true;
+      angularVelocityCoefficient = angularVelocityCoeff;
+    }
+  }
+  /**
+   * Setup the imuVelocity, this must be called after the IMU is created.
+   */
+  public void setupIMUVelocity()
+  {
+    imuVelocity = IMUVelocity.createIMUVelocity(imu);
   }
 
 }
diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java
index 45ad8e9e..e08dd421 100644
--- a/src/main/java/swervelib/imu/IMUVelocity.java
+++ b/src/main/java/swervelib/imu/IMUVelocity.java
@@ -1,8 +1,15 @@
 package swervelib.imu;
 
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.kauailabs.navx.frc.AHRS;
+
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.ADIS16448_IMU;
+import edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Notifier;
 
 public class IMUVelocity {
@@ -36,19 +43,6 @@ public class IMUVelocity {
    */
   private double velocity = 0.0;
 
-  /**
-   * Constructor for the IMU Velocity. Defaults to an IMU polling rate of 50hz 
-   * and 5 taps for moving average linear filter.
-   *
-   * @param gyro The SwerveIMU gyro.
-   */
-  public IMUVelocity(SwerveIMU gyro)
-  {
-    /* TODO: For now I am going to assume that gyros operate at 60hz or more by default  */
-    /* NAVX2.0 defaults to 60hz, Pigeon2 is 100hz  */
-    this(gyro, 1.0/60.0, 5);
-  }
-
   /**
    * Constructor for the IMU Velocity.
    *
@@ -69,6 +63,55 @@ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
     timestamp = HALUtil.getFPGATime();
   }
 
+  /**
+   * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible
+   * but will not exceed 100hz and will use 5 taps (supported IMUs are listed in swervelib's IMU folder). 
+   * Other gyroscopes will default to 50hz and 5 taps. For custom rates please use the IMUVelocity constructor.
+   *
+   * @param gyro The SwerveIMU gyro.
+   */
+  public static IMUVelocity createIMUVelocity(SwerveIMU gyro)
+  {
+    // ADIS16448_IMU ~200HZ:
+    // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277
+    if(gyro.getIMU() instanceof ADIS16448_IMU)
+    {
+      return new IMUVelocity(gyro, 1.0/100.0, 5);
+    }
+    // ADIS16470_IMU 200HZ
+    // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345
+    else if(gyro.getIMU() instanceof ADIS16470_IMU)
+    {
+      return new IMUVelocity(gyro, 1.0/100.0, 5);
+    }
+    // ADXRS450_Gyro 2000HZ?
+    // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31
+    else if(gyro.getIMU() instanceof ADXRS450_Gyro)
+    {
+      return new IMUVelocity(gyro, 1.0/100.0, 5);
+    }
+    // NAX (AHRS): 60HZ
+    // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61
+    else if(gyro.getIMU() instanceof AHRS)
+    {
+      return new IMUVelocity(gyro, 1.0/60.0, 5);
+    }
+    // Pigeon2 100HZ
+    // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf
+    else if(gyro.getIMU() instanceof Pigeon2)
+    {
+      return new IMUVelocity(gyro, 1.0/100.0, 5);
+    }
+    // Pigeon 100HZ
+    // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf
+    else if(gyro.getIMU() instanceof WPI_PigeonIMU)
+    {
+      return new IMUVelocity(gyro, 1.0/100.0, 5);
+    }
+    // defaults to 50hz and 5 taps
+    return new IMUVelocity(gyro, 1.0/50.0, 5);
+  }
+
   /**
    * Update the robot's rotational velocity based on the current gyro position.
    */

From 79d57bf6bec8a66fce6685b7fb18920d7df376c9 Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Thu, 3 Oct 2024 14:30:13 -0400
Subject: [PATCH 03/18] fixed IMU static factory, did the math, changed how
 auto optimizations are added

---
 .../swervedrive/SwerveSubsystem.java          |  2 +-
 src/main/java/swervelib/SwerveDrive.java      | 90 +++++++++----------
 src/main/java/swervelib/imu/IMUVelocity.java  | 38 ++++----
 3 files changed, 62 insertions(+), 68 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index 0b5ee486..55b66fcf 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -96,7 +96,7 @@ public SwerveSubsystem(File directory)
     }
     swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle.
     swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life.
-    swerveDrive.setAngularVelocityCompensation(true, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
+    swerveDrive.setAngularVelocityCompensation(true, true, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
     //Setting to true by default for testing, below is how I would push it
     //swerveDrive.setAngularVelocityCompensation(false, 0); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
     if (visionDriveTest)
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index 130f99f9..b4a8a39c 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -98,11 +98,21 @@ public class SwerveDrive
    * correction.
    */
   public        boolean                  chassisVelocityCorrection                       = true;
+  /**
+   * Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's
+   * correction during auto.
+   */
+  public        boolean                  autonomousChassisVelocityCorrection             = false;
   /**
    * Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
    * TODO: Still working on the explanation, I will have it ready soon (hopefully)
    */
   public        boolean                  angularVelocityCorrection                      = false;
+    /**
+   * Correct for skew that scales with angular velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)}
+   * during auto.
+   */
+  public        boolean                  autonomousAngularVelocityCorrection            = false;
   /**
    * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
    */
@@ -189,11 +199,6 @@ public SwerveDrive(
       imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
     }
 
-    if(angularVelocityCorrection)
-    {
-      setupIMUVelocity();
-    }
-
     this.swerveModules = config.modules;
 
     //    odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
@@ -500,9 +505,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
   {
     if(angularVelocityCorrection)
     {
-      // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now
-      // This multipler should be built into the imuVelocity.getVelocity() method and is almost certainly dependant on the timestep used
-      var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * 1000000 * angularVelocityCoefficient);
+      var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
       if(angularVelocity.getRadians() != 0.0){
         velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
                       velocity.vxMetersPerSecond,
@@ -644,28 +647,9 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo
    */
   public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
   {
-    SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
-    SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
-    SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
-
-    setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
-  }
-    /**
-   * Set chassis speeds with closed-loop velocity control, using the optimizations present 
-   * in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. This may improve
-   * performance during autonomous, if the optimizations reduce your bot's skew in teleop then 
-   * they should help in autonomous, think of this as a feedforward that reduces the correction 
-   * needed from PID due to skew.
-   *
-   * @param chassisSpeeds Chassis speeds to set.
-   */
-  public void setChassisSpeedsWithDriveOptimizations(ChassisSpeeds chassisSpeeds)
-  {
-    if(angularVelocityCorrection)
+    if(autonomousAngularVelocityCorrection)
     {
-      // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now
-      // This multipler should be built into the imuVelocity.getVelocity() function and is almost certainly dependant on the timestep used
-      var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * 1000000 * angularVelocityCoefficient);
+      var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
       if(angularVelocity.getRadians() != 0.0){
         chassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(
                       chassisSpeeds.vxMetersPerSecond,
@@ -678,7 +662,7 @@ public void setChassisSpeedsWithDriveOptimizations(ChassisSpeeds chassisSpeeds)
 
     // Thank you to Jared Russell FRC254 for Open Loop Compensation Code
     // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
-    if (chassisVelocityCorrection)
+    if (autonomousChassisVelocityCorrection)
     {
       chassisSpeeds = ChassisSpeeds.discretize(chassisSpeeds, discretizationdtSeconds);
     }
@@ -1278,7 +1262,7 @@ public void setCosineCompensator(boolean enabled)
   }
 
   /**
-   * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction
+   * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
    *
    * @param enable    Enable chassis velocity correction, which will use
    *                  {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following.
@@ -1291,33 +1275,49 @@ public void setChassisDiscretization(boolean enable, double dtSeconds)
       chassisVelocityCorrection = enable;
       discretizationdtSeconds = dtSeconds;
     }
-
   }
 
   /**
-   * Enables angular velocity correction and sets the angular velocity coefficient
+   * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto
    *
-   * @param enable               Enables angular velocity correction.
-   * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15, although higher values may be needed.
-   *                             Start with a value of 0.1.
-   *                             When enabling for the first time if the skew is significantly worse try inverting the value.
-   *                             Tune by moving in a straight line while rotating. Change the value until you are visually happy with the skew.
-   *                             Ensure your tune works with different translational and rotational magnitudes.
+   * @param useInTeleop    Enable chassis velocity correction, which will use
+   *                       {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
+   * @param useInAuto      Enable chassis velocity correction, which will use
+   *                       {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
+   * @param dtSeconds      The duration of the timestep the speeds should be applied for.
    */
-  public void setAngularVelocityCompensation(boolean enable, double angularVelocityCoeff)
+  public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
   {
     if(!SwerveDriveTelemetry.isSimulation)
     {
-      angularVelocityCorrection = true;
-      angularVelocityCoefficient = angularVelocityCoeff;
+      chassisVelocityCorrection = useInTeleop;
+      autonomousChassisVelocityCorrection = useInAuto;
+      discretizationdtSeconds = dtSeconds;
     }
   }
+
   /**
-   * Setup the imuVelocity, this must be called after the IMU is created.
+   * Enables angular velocity correction in telop and/or autonomous and sets the angular velocity coefficient for both modes
+   *
+   * @param useInTeleop          Enables angular velocity correction in teleop.
+   * @param useInAuto            Enables angular velocity correction in autonomous.
+   * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15.
+   *                             Start with a value of 0.1, test in teleop.
+   *                             When enabling for the first time if the skew is significantly worse try inverting the value.
+   *                             Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick. 
+   *                             Change the value until you are visually happy with the skew.
+   *                             Ensure your tune works with different translational and rotational magnitudes.
+   *                             If this reduces skew in teleop, it may improve auto.
    */
-  public void setupIMUVelocity()
+  public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff)
   {
-    imuVelocity = IMUVelocity.createIMUVelocity(imu);
+    if(!SwerveDriveTelemetry.isSimulation)
+    {
+      imuVelocity = IMUVelocity.createIMUVelocity(imu);
+      angularVelocityCorrection = useInTeleop;
+      autonomousAngularVelocityCorrection = useInAuto;
+      angularVelocityCoefficient = angularVelocityCoeff;
+    }
   }
 
 }
diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java
index e08dd421..f6e3ca3b 100644
--- a/src/main/java/swervelib/imu/IMUVelocity.java
+++ b/src/main/java/swervelib/imu/IMUVelocity.java
@@ -1,15 +1,8 @@
 package swervelib.imu;
 
-import com.ctre.phoenix.sensors.WPI_PigeonIMU;
-import com.ctre.phoenix6.hardware.Pigeon2;
-import com.kauailabs.navx.frc.AHRS;
-
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.ADIS16448_IMU;
-import edu.wpi.first.wpilibj.ADIS16470_IMU;
-import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Notifier;
 
 public class IMUVelocity {
@@ -72,44 +65,44 @@ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
    */
   public static IMUVelocity createIMUVelocity(SwerveIMU gyro)
   {
+    double desiredNotifierPeriod = 1.0/50.0;
     // ADIS16448_IMU ~200HZ:
     // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277
-    if(gyro.getIMU() instanceof ADIS16448_IMU)
+    if(gyro instanceof ADIS16448Swerve)
     {
-      return new IMUVelocity(gyro, 1.0/100.0, 5);
+      desiredNotifierPeriod = 1.0/100.0;
     }
     // ADIS16470_IMU 200HZ
     // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345
-    else if(gyro.getIMU() instanceof ADIS16470_IMU)
+    else if(gyro instanceof ADIS16470Swerve)
     {
-      return new IMUVelocity(gyro, 1.0/100.0, 5);
+      desiredNotifierPeriod = 1.0/100.0;
     }
     // ADXRS450_Gyro 2000HZ?
     // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31
-    else if(gyro.getIMU() instanceof ADXRS450_Gyro)
+    else if(gyro instanceof ADXRS450Swerve)
     {
-      return new IMUVelocity(gyro, 1.0/100.0, 5);
+      desiredNotifierPeriod = 1.0/100.0;
     }
     // NAX (AHRS): 60HZ
     // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61
-    else if(gyro.getIMU() instanceof AHRS)
+    else if(gyro instanceof NavXSwerve)
     {
-      return new IMUVelocity(gyro, 1.0/60.0, 5);
+      desiredNotifierPeriod = 1.0/60.0;
     }
     // Pigeon2 100HZ
     // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf
-    else if(gyro.getIMU() instanceof Pigeon2)
+    else if(gyro instanceof Pigeon2Swerve)
     {
-      return new IMUVelocity(gyro, 1.0/100.0, 5);
+      desiredNotifierPeriod = 1.0/100.0;
     }
     // Pigeon 100HZ
     // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf
-    else if(gyro.getIMU() instanceof WPI_PigeonIMU)
+    else if(gyro instanceof PigeonSwerve)
     {
-      return new IMUVelocity(gyro, 1.0/100.0, 5);
+      desiredNotifierPeriod = 1.0/100.0;
     }
-    // defaults to 50hz and 5 taps
-    return new IMUVelocity(gyro, 1.0/50.0, 5);
+    return new IMUVelocity(gyro, desiredNotifierPeriod, 5);
   }
 
   /**
@@ -133,10 +126,11 @@ private void update()
 
   /**
    * Get the robot's angular velocity based on averaged meaasurements from the IMU.
+   * Velocity is multiplied by 1e+6 (1,000,000) because the timestamps are in microseconds.
    *
    * @return robot's angular velocity in rads/s as a double.
    */
   public synchronized double getVelocity() {
-    return velocity;
+    return velocity * 1e+6;
   }
 }

From 886f33ead28a0d62e1c4558c415cbfcec2573eca Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Thu, 3 Oct 2024 17:38:36 -0400
Subject: [PATCH 04/18] added custom filter to reduce calculations

---
 src/main/java/swervelib/SwerveDrive.java      | 88 +++++++++++--------
 src/main/java/swervelib/imu/IMUVelocity.java  | 13 +--
 .../math/IMULinearMovingAverageFilter.java    | 47 ++++++++++
 3 files changed, 104 insertions(+), 44 deletions(-)
 create mode 100644 src/main/java/swervelib/math/IMULinearMovingAverageFilter.java

diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index b4a8a39c..a4c8914f 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -105,7 +105,6 @@ public class SwerveDrive
   public        boolean                  autonomousChassisVelocityCorrection             = false;
   /**
    * Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
-   * TODO: Still working on the explanation, I will have it ready soon (hopefully)
    */
   public        boolean                  angularVelocityCorrection                      = false;
     /**
@@ -503,25 +502,8 @@ public void drive(
    */
   public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
   {
-    if(angularVelocityCorrection)
-    {
-      var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
-      if(angularVelocity.getRadians() != 0.0){
-        velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
-                      velocity.vxMetersPerSecond,
-                      velocity.vyMetersPerSecond,
-                      velocity.omegaRadiansPerSecond,
-                      getOdometryHeading());
-        velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
-      }
-    }
 
-    // Thank you to Jared Russell FRC254 for Open Loop Compensation Code
-    // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
-    if (chassisVelocityCorrection)
-    {
-      velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
-    }
+    velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection);
 
     // Heading Angular Velocity Deadband, might make a configuration option later.
     // Originally made by Team 1466 Webb Robotics.
@@ -647,25 +629,8 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo
    */
   public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
   {
-    if(autonomousAngularVelocityCorrection)
-    {
-      var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
-      if(angularVelocity.getRadians() != 0.0){
-        chassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(
-                      chassisSpeeds.vxMetersPerSecond,
-                      chassisSpeeds.vyMetersPerSecond,
-                      chassisSpeeds.omegaRadiansPerSecond,
-                      getOdometryHeading());
-        chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, getOdometryHeading().plus(angularVelocity));
-      }
-    }
 
-    // Thank you to Jared Russell FRC254 for Open Loop Compensation Code
-    // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
-    if (autonomousChassisVelocityCorrection)
-    {
-      chassisSpeeds = ChassisSpeeds.discretize(chassisSpeeds, discretizationdtSeconds);
-    }
+    chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection);
 
     SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
     SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
@@ -1297,7 +1262,8 @@ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, dou
   }
 
   /**
-   * Enables angular velocity correction in telop and/or autonomous and sets the angular velocity coefficient for both modes
+   * Enables angular velocity skew correction in telop and/or autonomous
+   * and sets the angular velocity coefficient for both modes
    *
    * @param useInTeleop          Enables angular velocity correction in teleop.
    * @param useInAuto            Enables angular velocity correction in autonomous.
@@ -1320,4 +1286,50 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut
     }
   }
 
+  /**
+   * Correct for skew that worsens as angular velocity increases
+   * 
+   * @param velocity  The chassis speeds to set the robot to achieve.
+   * @return {@link ChassisSpeeds} of the robot after angular velocity skew correction.
+   */
+  public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
+  {
+    var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
+      if(angularVelocity.getRadians() != 0.0){
+        velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
+                      velocity.vxMetersPerSecond,
+                      velocity.vyMetersPerSecond,
+                      velocity.omegaRadiansPerSecond,
+                      getOdometryHeading());
+        velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
+      }
+    return velocity;
+  }
+
+  /**
+   * Enable desired drive corrections
+   * 
+   * @param velocity                         The chassis speeds to set the robot to achieve.
+   * @param useChassisDiscretize             Correct chassis velocity using 254's correction.
+   * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew.
+   * @return                                 The chassis speeds after optimizations.
+   */
+  public ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection)
+  {
+    
+    if(useAngularVelocitySkewCorrection)
+    {
+      velocity = angularVelocitySkewCorrection(velocity);
+    }
+
+    // Thank you to Jared Russell FRC254 for Open Loop Compensation Code
+    // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
+    if (uesChassisDiscretize)
+    {
+      velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
+    }
+    
+    return velocity;
+  }
+
 }
diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java
index f6e3ca3b..f81b5822 100644
--- a/src/main/java/swervelib/imu/IMUVelocity.java
+++ b/src/main/java/swervelib/imu/IMUVelocity.java
@@ -1,9 +1,9 @@
 package swervelib.imu;
 
 import edu.wpi.first.hal.HALUtil;
-import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.Notifier;
+import swervelib.math.IMULinearMovingAverageFilter;
 
 public class IMUVelocity {
   /**
@@ -11,9 +11,10 @@ public class IMUVelocity {
    */
   private final SwerveIMU gyro;
   /**
-   * Linear filter used to calculate velocity.
+   * Linear filter used to calculate velocity, we use a custom filter class
+   * to prevent unwanted operations.
    */
-  private final LinearFilter velocityFilter;
+  private final IMULinearMovingAverageFilter velocityFilter;
   /**
    * WPILib {@link Notifier} to keep IMU velocity up to date.
    */
@@ -50,7 +51,7 @@ public class IMUVelocity {
   public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
   {
     this.gyro = gyro;
-    velocityFilter = LinearFilter.movingAverage(averagingTaps);
+    velocityFilter = new IMULinearMovingAverageFilter(averagingTaps);
     notifier = new Notifier(this::update);
     notifier.startPeriodic(periodSeconds);
     timestamp = HALUtil.getFPGATime();
@@ -115,8 +116,7 @@ private void update()
 
     synchronized (this) {
       if (!firstCycle) {
-        velocity = velocityFilter.calculate(
-            (newPosition.minus(position).getRadians()) / (newTimestamp - timestamp));
+        velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp));
         }
       firstCycle = false;
       timestamp = newTimestamp;
@@ -131,6 +131,7 @@ private void update()
    * @return robot's angular velocity in rads/s as a double.
    */
   public synchronized double getVelocity() {
+    velocity = velocityFilter.calculate();
     return velocity * 1e+6;
   }
 }
diff --git a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java
new file mode 100644
index 00000000..552ae620
--- /dev/null
+++ b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java
@@ -0,0 +1,47 @@
+package swervelib.math;
+
+import edu.wpi.first.util.DoubleCircularBuffer;
+
+/**
+ * A linear filter that does not calculate() each time a value is added to
+ * the DoubleCircularBuffer.
+ */
+public class IMULinearMovingAverageFilter {
+  private final DoubleCircularBuffer m_inputs;
+  private final double m_inputGain;
+    
+ /**
+  * Construct a linear moving average fitler
+  * @param bufferLength The number of values to average across
+  */
+  public IMULinearMovingAverageFilter(int bufferLength)
+  {
+    m_inputs = new DoubleCircularBuffer(bufferLength);
+    m_inputGain = 1.0 / bufferLength;
+  }
+
+ /**
+  * Add a value to the DoubleCircularBuffer
+  * @param input
+  */
+  public void addValue(double input)
+  {
+    m_inputs.addFirst(input);
+  }
+
+ /**
+  * Calculate the average of the samples in the buffer
+  * @return The average of the values in the buffer
+  */
+  public double calculate()
+  {
+    double returnVal = 0.0;
+
+      for(int i = 0; i < m_inputs.size(); i++)
+      {
+        returnVal += m_inputs.get(i) * m_inputGain;
+      }
+        
+      return returnVal;
+  }
+}

From f1b3c8aad1d8d5118d003a911b62ee41a5cf98a6 Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Fri, 4 Oct 2024 15:07:41 -0400
Subject: [PATCH 05/18] formatting

---
 src/main/java/swervelib/imu/IMUVelocity.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java
index f6e3ca3b..78a64d93 100644
--- a/src/main/java/swervelib/imu/IMUVelocity.java
+++ b/src/main/java/swervelib/imu/IMUVelocity.java
@@ -68,37 +68,37 @@ public static IMUVelocity createIMUVelocity(SwerveIMU gyro)
     double desiredNotifierPeriod = 1.0/50.0;
     // ADIS16448_IMU ~200HZ:
     // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277
-    if(gyro instanceof ADIS16448Swerve)
+    if (gyro instanceof ADIS16448Swerve)
     {
       desiredNotifierPeriod = 1.0/100.0;
     }
     // ADIS16470_IMU 200HZ
     // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345
-    else if(gyro instanceof ADIS16470Swerve)
+    else if (gyro instanceof ADIS16470Swerve)
     {
       desiredNotifierPeriod = 1.0/100.0;
     }
     // ADXRS450_Gyro 2000HZ?
     // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31
-    else if(gyro instanceof ADXRS450Swerve)
+    else if (gyro instanceof ADXRS450Swerve)
     {
       desiredNotifierPeriod = 1.0/100.0;
     }
     // NAX (AHRS): 60HZ
     // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61
-    else if(gyro instanceof NavXSwerve)
+    else if (gyro instanceof NavXSwerve)
     {
       desiredNotifierPeriod = 1.0/60.0;
     }
     // Pigeon2 100HZ
     // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf
-    else if(gyro instanceof Pigeon2Swerve)
+    else if (gyro instanceof Pigeon2Swerve)
     {
       desiredNotifierPeriod = 1.0/100.0;
     }
     // Pigeon 100HZ
     // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf
-    else if(gyro instanceof PigeonSwerve)
+    else if (gyro instanceof PigeonSwerve)
     {
       desiredNotifierPeriod = 1.0/100.0;
     }

From c5f1cf17c2b828db61e5245a8f9b40292a8c28a3 Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Fri, 4 Oct 2024 15:08:58 -0400
Subject: [PATCH 06/18] privated movementOptimizations

---
 src/main/java/swervelib/SwerveDrive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index a4c8914f..44024773 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -1314,7 +1314,7 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
    * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew.
    * @return                                 The chassis speeds after optimizations.
    */
-  public ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection)
+  private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection)
   {
     
     if(useAngularVelocitySkewCorrection)

From 2f3b07b18e2e0ff4082bf07d65c907757caca70d Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 14:57:43 -0400
Subject: [PATCH 07/18] rebased changes

---
 .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java  | 2 --
 src/main/java/swervelib/SwerveDrive.java                        | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index 55b66fcf..675938b3 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -97,8 +97,6 @@ public SwerveSubsystem(File directory)
     swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle.
     swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life.
     swerveDrive.setAngularVelocityCompensation(true, true, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
-    //Setting to true by default for testing, below is how I would push it
-    //swerveDrive.setAngularVelocityCompensation(false, 0); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
     if (visionDriveTest)
     {
       setupPhotonVision();
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index 44024773..280dc1a0 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -1310,7 +1310,7 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
    * Enable desired drive corrections
    * 
    * @param velocity                         The chassis speeds to set the robot to achieve.
-   * @param useChassisDiscretize             Correct chassis velocity using 254's correction.
+   * @param uesChassisDiscretize             Correct chassis velocity using 254's correction.
    * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew.
    * @return                                 The chassis speeds after optimizations.
    */

From d53d0bb6e7189c8c574cd7cb89f004610fa8495c Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 15:32:36 -0400
Subject: [PATCH 08/18] synced with dev

---
 src/main/java/swervelib/math/SwerveMath.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java
index b77838c0..ca278a9e 100644
--- a/src/main/java/swervelib/math/SwerveMath.java
+++ b/src/main/java/swervelib/math/SwerveMath.java
@@ -136,7 +136,7 @@ public static double calculateDegreesPerSteeringRotation(
   public static double calculateMaxAngularVelocity(
       double maxSpeed, double furthestModuleX, double furthestModuleY)
   {
-    return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
+    return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians();
   }
 
   /**

From fff92c6620682ce9727a9d83ea33d084e1f66281 Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 16:04:25 -0400
Subject: [PATCH 09/18] added optimizations to spark max configuration from top
 level teams

---
 .../java/swervelib/motors/SparkMaxSwerve.java  | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
index a2c421e1..6ac197b8 100644
--- a/src/main/java/swervelib/motors/SparkMaxSwerve.java
+++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -224,12 +224,30 @@ public void configureIntegratedEncoder(double positionConversionFactor)
     {
       configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
       configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
+      // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
+      // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
+      // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
+      // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
+      // This value was taken from:
+      // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
+      // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
+      configureSparkMax(() -> encoder.setMeasurementPeriod(10));
+      configureSparkMax(() -> encoder.setAverageDepth(2));
 
       // Taken from
       // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
+      // Unused frames can be set to 65535 to decrease CAN ultilization.
       configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
     } else
     {
+      // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
+      // This needs to be set to 20ms or under to properly update the swerve module position for odometry
+      // Configuration taken from 3005, the team who helped develop the Max Swerve:
+      // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
+      // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. 
+      // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
+      // with limited testing 19ms did not return the same value while the module was constatntly rotating. 
+      configureCANStatusFrames(100, 20, 50, 200, 20, 8, 50);
       configureSparkMax(() -> {
         if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
         {

From cbd9ba8f61af1b390f8a7b0172453a5470bd80f6 Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 17:15:08 -0400
Subject: [PATCH 10/18] checked for motor type before CANStatusConfig

---
 src/main/java/swervelib/motors/SparkMaxSwerve.java | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
index 6ac197b8..f7383d3d 100644
--- a/src/main/java/swervelib/motors/SparkMaxSwerve.java
+++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -11,6 +11,8 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkPIDController;
+
+import edu.wpi.first.wpilibj.AnalogEncoder;
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -246,8 +248,16 @@ public void configureIntegratedEncoder(double positionConversionFactor)
       // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
       // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. 
       // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
-      // with limited testing 19ms did not return the same value while the module was constatntly rotating. 
-      configureCANStatusFrames(100, 20, 50, 200, 20, 8, 50);
+      // with limited testing 19ms did not return the same value while the module was constatntly rotating.
+      if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+      {
+        configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50);
+      }
+      // Needs to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
+      else if(absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
+      {
+        configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
+      }
       configureSparkMax(() -> {
         if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
         {

From 9448a16e6976a871d536ae0ddbc2b77a25b07305 Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 17:16:34 -0400
Subject: [PATCH 11/18] spelling

---
 src/main/java/swervelib/motors/SparkMaxSwerve.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
index f7383d3d..10112143 100644
--- a/src/main/java/swervelib/motors/SparkMaxSwerve.java
+++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -253,7 +253,7 @@ public void configureIntegratedEncoder(double positionConversionFactor)
       {
         configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50);
       }
-      // Needs to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
+      // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
       else if(absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
       {
         configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);

From b9ec36e9c8a7143d438d510ffed19ea431e6076c Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 17:32:42 -0400
Subject: [PATCH 12/18] bad import

---
 src/main/java/swervelib/motors/SparkMaxSwerve.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
index 10112143..064f6a9f 100644
--- a/src/main/java/swervelib/motors/SparkMaxSwerve.java
+++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -12,7 +12,6 @@
 import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkPIDController;
 
-import edu.wpi.first.wpilibj.AnalogEncoder;
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;

From e808e39c93cb3be2018bfdfcbd0272ddb1001fca Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 17:33:12 -0400
Subject: [PATCH 13/18] bad import

---
 src/main/java/swervelib/motors/SparkMaxSwerve.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
index 064f6a9f..0f014173 100644
--- a/src/main/java/swervelib/motors/SparkMaxSwerve.java
+++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -11,7 +11,6 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkPIDController;
-
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;

From 46704776817a638d042e65c2ff3019676a862349 Mon Sep 17 00:00:00 2001
From: Curt <clrozeboom@gmail.com>
Date: Sat, 7 Sep 2024 16:58:40 -0400
Subject: [PATCH 14/18] Swerve configuration test improvements

Robot.java - Test mode calls RobotContainer.setDriveMode to configure different controls
RobotContainer.java - Move drive options to class-level so they are accessible in configureBindings, added a new binding map for Test mode that is useful for configuring swerve
SwerveSubsystem.java - Added various helper functions that can be called from RobotContainer to set up buttons for testing swerve
SwerveDriveTest.java - Fix angle motor SysId test issue with reading the wrong encoder
---
 src/main/java/frc/robot/Robot.java            |  10 +-
 src/main/java/frc/robot/RobotContainer.java   | 133 ++++++++++--------
 .../swervedrive/SwerveSubsystem.java          |  77 +++++++++-
 src/main/java/swervelib/SwerveDriveTest.java  |   4 +-
 4 files changed, 152 insertions(+), 72 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 94e6ddb6..52a18c40 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -125,6 +125,8 @@ public void teleopInit()
     if (m_autonomousCommand != null)
     {
       m_autonomousCommand.cancel();
+    } else {
+      CommandScheduler.getInstance().cancelAll();
     }
     m_robotContainer.setDriveMode();
     m_robotContainer.setMotorBrake(true);
@@ -143,13 +145,7 @@ public void testInit()
   {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
-    try
-    {
-      new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve"));
-    } catch (IOException e)
-    {
-      throw new RuntimeException(e);
-    }
+    m_robotContainer.setDriveMode();
   }
 
   /**
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d44e8b4a..92017828 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -8,8 +8,11 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -32,6 +35,50 @@ public class RobotContainer
   // The robot's subsystems and commands are defined here...
   private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
                                                                          "swerve/neo"));
+  // Applies deadbands and inverts controls because joysticks
+  // are back-right positive while robot
+  // controls are front-left positive
+  // left stick controls translation
+  // right stick controls the rotational velocity 
+  // buttons are quick rotation positions to different ways to face
+  // WARNING: default buttons are on the same buttons as the ones defined in configureBindings
+  AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
+                                                                  () -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
+                                                                                                OperatorConstants.LEFT_Y_DEADBAND),
+                                                                  () -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
+                                                                                                OperatorConstants.LEFT_X_DEADBAND),
+                                                                  () -> -MathUtil.applyDeadband(driverXbox.getRightX(),
+                                                                                                OperatorConstants.RIGHT_X_DEADBAND),
+                                                                  driverXbox.getHID()::getYButtonPressed,
+                                                                  driverXbox.getHID()::getAButtonPressed,
+                                                                  driverXbox.getHID()::getXButtonPressed,
+                                                                  driverXbox.getHID()::getBButtonPressed);
+
+  // Applies deadbands and inverts controls because joysticks
+  // are back-right positive while robot
+  // controls are front-left positive
+  // left stick controls translation
+  // right stick controls the desired angle NOT angular rotation
+  Command driveFieldOrientedDirectAngle = drivebase.driveCommand(
+      () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
+      () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
+      () -> driverXbox.getRightX(),
+      () -> driverXbox.getRightY());
+
+  // Applies deadbands and inverts controls because joysticks
+  // are back-right positive while robot
+  // controls are front-left positive
+  // left stick controls translation
+  // right stick controls the angular velocity of the robot
+  Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand(
+      () -> MathUtil.applyDeadband(driverXbox.getLeftY() * -1, OperatorConstants.LEFT_Y_DEADBAND),
+      () -> MathUtil.applyDeadband(driverXbox.getLeftX() * -1, OperatorConstants.LEFT_X_DEADBAND),
+      () -> driverXbox.getRightX() * -1);
+
+  Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand(
+      () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
+      () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
+      () -> driverXbox.getRawAxis(2));
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -40,54 +87,6 @@ public RobotContainer()
   {
     // Configure the trigger bindings
     configureBindings();
-
-    // Applies deadbands and inverts controls because joysticks
-    // are back-right positive while robot
-    // controls are front-left positive
-    // left stick controls translation
-    // right stick controls the rotational velocity 
-    // buttons are quick rotation positions to different ways to face
-    // WARNING: default buttons are on the same buttons as the ones defined in configureBindings
-    AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
-                                                                   () -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
-                                                                                                 OperatorConstants.LEFT_Y_DEADBAND),
-                                                                   () -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
-                                                                                                 OperatorConstants.LEFT_X_DEADBAND),
-                                                                   () -> -MathUtil.applyDeadband(driverXbox.getRightX(),
-                                                                                                 OperatorConstants.RIGHT_X_DEADBAND),
-                                                                   driverXbox.getHID()::getYButtonPressed,
-                                                                   driverXbox.getHID()::getAButtonPressed,
-                                                                   driverXbox.getHID()::getXButtonPressed,
-                                                                   driverXbox.getHID()::getBButtonPressed);
-
-    // Applies deadbands and inverts controls because joysticks
-    // are back-right positive while robot
-    // controls are front-left positive
-    // left stick controls translation
-    // right stick controls the desired angle NOT angular rotation
-    Command driveFieldOrientedDirectAngle = drivebase.driveCommand(
-        () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
-        () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
-        () -> driverXbox.getRightX(),
-        () -> driverXbox.getRightY());
-
-    // Applies deadbands and inverts controls because joysticks
-    // are back-right positive while robot
-    // controls are front-left positive
-    // left stick controls translation
-    // right stick controls the angular velocity of the robot
-    Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand(
-        () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
-        () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
-        () -> driverXbox.getRightX() * 0.5);
-
-    Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand(
-        () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
-        () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
-        () -> driverXbox.getRawAxis(2));
-
-    drivebase.setDefaultCommand(
-        !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim);
   }
 
   /**
@@ -99,16 +98,32 @@ public RobotContainer()
    */
   private void configureBindings()
   {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-
-    driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-    driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
-    driverXbox.b().whileTrue(
-        Commands.deferredProxy(() -> drivebase.driveToPose(
-                                   new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
-                              ));
-    driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2));
-    // driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+    if (DriverStation.isTest()) {
+      driverXbox.a().onTrue(drivebase.angleMotorPValueCommand());
+      driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand());
+      driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+      driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2));
+      driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
+      driverXbox.back().whileTrue(drivebase.centerModulesCommand());
+      driverXbox.leftBumper().onTrue(Commands.none());
+      driverXbox.rightBumper().onTrue(Commands.none());
+      drivebase.setDefaultCommand(
+        !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim);
+    } else {
+      driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
+      driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
+      driverXbox.b().whileTrue(
+          Commands.deferredProxy(() -> drivebase.driveToPose(
+                                    new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
+                                ));
+      driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2));
+      driverXbox.start().whileTrue(Commands.none());
+      driverXbox.back().whileTrue(Commands.none());
+      driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+      driverXbox.rightBumper().onTrue(Commands.none());
+      drivebase.setDefaultCommand(
+        !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim);
+    }
   }
 
   /**
@@ -124,7 +139,7 @@ public Command getAutonomousCommand()
 
   public void setDriveMode()
   {
-    //drivebase.setDefaultCommand();
+    configureBindings();
   }
 
   public void setMotorBrake(boolean brake)
diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index 675938b3..c2f7d392 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -4,14 +4,23 @@
 
 package frc.robot.subsystems.swervedrive;
 
+import java.io.File;
+import java.util.Arrays;
+import java.util.function.DoubleSupplier;
+
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.ReplanningConfig;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -23,19 +32,18 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.Constants;
 import frc.robot.Constants.AutonConstants;
-import java.io.File;
-import java.util.function.DoubleSupplier;
-import org.photonvision.PhotonCamera;
-import org.photonvision.targeting.PhotonPipelineResult;
 import swervelib.SwerveController;
 import swervelib.SwerveDrive;
 import swervelib.SwerveDriveTest;
 import swervelib.math.SwerveMath;
+import swervelib.parser.PIDFConfig;
 import swervelib.parser.SwerveControllerConfiguration;
 import swervelib.parser.SwerveDriveConfiguration;
 import swervelib.parser.SwerveParser;
@@ -352,6 +360,67 @@ public Command sysIdAngleMotorCommand()
         3.0, 5.0, 3.0);
   }
 
+  /**
+   * Returns a Command that sets the P value of the angle motors to the value on the SmartDashboard and updates the PIDF
+   * configuration of each module. The command is run only once and is intended to be used for characterizing the angle
+   * motors' P value. The P value is read from the SmartDashboard and if it is not present, a default value of 0 is used.
+   *
+   * @return a Command that reads the P value of the angle motors from the SmartDashboard and updates the PIDF configuration of each module
+   */ 
+  public Command angleMotorPValueCommand() {
+    return Commands.runOnce(() -> {
+        SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p);
+        swerveDrive.getModuleMap().values().forEach(module -> {
+          module.getAngleMotor().configurePIDF(new PIDFConfig(SmartDashboard.getNumber("Angle Motor P", 0), 0));
+          module.getAngleMotor().burnFlash();
+        });
+      }, this);
+  }
+
+  /**
+   * Returns a Command that centers the modules of the SwerveDrive subsystem.
+   *
+   * @return a Command that centers the modules of the SwerveDrive subsystem
+   */
+  public Command centerModulesCommand() {
+    return run(() -> Arrays.asList(swerveDrive.getModules())
+      .forEach(it -> it.setAngle(0.0)));
+  }
+
+  /**
+   * Returns a Command that drives the swerve drive to a specific distance at a given speed.
+   *
+   * @param distanceInMeters the distance to drive in meters
+   * @param speedInMetersPerSecond the speed at which to drive in meters per second
+   * @return a Command that drives the swerve drive to a specific distance at a given speed
+   */  
+  public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) {
+    return Commands.deferredProxy(
+        () -> Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this)
+        .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > distanceInMeters)
+        );
+  }
+  
+  /**
+   * Sets the maximum speed of the swerve drive.
+   *
+   * @param maximumSpeed the maximum speed to set for the swerve drive in meters per second
+   */
+  public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) {
+    swerveDrive.setMaximumSpeed(maximumSpeedInMetersPerSecond, false, swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage);
+  }
+
+  /**
+   * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object.
+   *
+   * @param  kS  the static gain of the feedforward
+   * @param  kV  the velocity gain of the feedforward
+   * @param  kA  the acceleration gain of the feedforward
+   */  
+  public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
+    swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
+  }
+
   /**
    * Command to drive the robot using translative values and heading as angular velocity.
    *
diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java
index 848852c6..a3d71dfe 100644
--- a/src/main/java/swervelib/SwerveDriveTest.java
+++ b/src/main/java/swervelib/SwerveDriveTest.java
@@ -356,8 +356,8 @@ public static void logAngularMotorVoltage(SwerveModule module, SysIdRoutineLog l
   public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied)
   {
     double power    = powerSupplied.get();
-    double angle    = module.getAbsolutePosition();
-    double velocity = module.getAbsoluteEncoder().getVelocity();
+    double angle    = module.getAngleMotor().getPosition();
+    double velocity = module.getAngleMotor().getVelocity();
     SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power);
     SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle);
     SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity",

From 00fc7e89211e4dde067f036bc4a57d0feacf1f9f Mon Sep 17 00:00:00 2001
From: Curt <clrozeboom@gmail.com>
Date: Sat, 7 Sep 2024 17:26:54 -0400
Subject: [PATCH 15/18] Change place where Angle Motor P is added to NT

---
 .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java  | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index c2f7d392..946a2b44 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -368,8 +368,8 @@ public Command sysIdAngleMotorCommand()
    * @return a Command that reads the P value of the angle motors from the SmartDashboard and updates the PIDF configuration of each module
    */ 
   public Command angleMotorPValueCommand() {
+    SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p);
     return Commands.runOnce(() -> {
-        SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p);
         swerveDrive.getModuleMap().values().forEach(module -> {
           module.getAngleMotor().configurePIDF(new PIDFConfig(SmartDashboard.getNumber("Angle Motor P", 0), 0));
           module.getAngleMotor().burnFlash();

From 6b856d381e5276c9f9e0774a9500308cdbd513e9 Mon Sep 17 00:00:00 2001
From: Curt <clrozeboom@gmail.com>
Date: Sat, 14 Sep 2024 15:20:50 -0400
Subject: [PATCH 16/18] Remove Angle Motor configuring

---
 src/main/java/frc/robot/RobotContainer.java     |  1 -
 .../subsystems/swervedrive/SwerveSubsystem.java | 17 -----------------
 2 files changed, 18 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 92017828..56b25cea 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -99,7 +99,6 @@ public RobotContainer()
   private void configureBindings()
   {
     if (DriverStation.isTest()) {
-      driverXbox.a().onTrue(drivebase.angleMotorPValueCommand());
       driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand());
       driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
       driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2));
diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index 946a2b44..d1e18842 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -360,23 +360,6 @@ public Command sysIdAngleMotorCommand()
         3.0, 5.0, 3.0);
   }
 
-  /**
-   * Returns a Command that sets the P value of the angle motors to the value on the SmartDashboard and updates the PIDF
-   * configuration of each module. The command is run only once and is intended to be used for characterizing the angle
-   * motors' P value. The P value is read from the SmartDashboard and if it is not present, a default value of 0 is used.
-   *
-   * @return a Command that reads the P value of the angle motors from the SmartDashboard and updates the PIDF configuration of each module
-   */ 
-  public Command angleMotorPValueCommand() {
-    SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p);
-    return Commands.runOnce(() -> {
-        swerveDrive.getModuleMap().values().forEach(module -> {
-          module.getAngleMotor().configurePIDF(new PIDFConfig(SmartDashboard.getNumber("Angle Motor P", 0), 0));
-          module.getAngleMotor().burnFlash();
-        });
-      }, this);
-  }
-
   /**
    * Returns a Command that centers the modules of the SwerveDrive subsystem.
    *

From bf57686f861f7ebc4bd28da5b2b1eafddf8ac1cd Mon Sep 17 00:00:00 2001
From: yapplejack <jacklf@umich.edu>
Date: Wed, 2 Oct 2024 15:17:00 -0400
Subject: [PATCH 17/18] changed input to desaturateWheelSpeeds from
 getRobotVelocity() to desiredChaassisSpeeds

---
 src/main/java/swervelib/SwerveDrive.java | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index 280dc1a0..687f4f75 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -537,7 +537,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
     // Calculate required module states via kinematics
     SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters);
 
-    setRawModuleStates(swerveModuleStates, isOpenLoop);
+    setRawModuleStates(swerveModuleStates, velocity, isOpenLoop);
   }
 
   /**
@@ -587,14 +587,15 @@ public double getMaximumAngularVelocity()
    * Set the module states (azimuth and velocity) directly.
    *
    * @param desiredStates A list of SwerveModuleStates to send to the modules.
+   * @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve.
    * @param isOpenLoop    Whether to use closed-loop velocity control. Set to true to disable closed-loop.
    */
-  private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
+  private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, boolean isOpenLoop)
   {
     // Desaturates wheel speeds
     if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
     {
-      SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
+      SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed,
                                                   maxSpeedMPS,
                                                   attainableMaxTranslationalSpeedMetersPerSecond,
                                                   attainableMaxRotationalVelocityRadiansPerSecond);
@@ -612,14 +613,23 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpe
 
   /**
    * Set the module states (azimuth and velocity) directly. Used primarily for auto paths.
+   * Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates, 
+   * ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, 
+   * double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
    *
    * @param desiredStates A list of SwerveModuleStates to send to the modules.
    * @param isOpenLoop    Whether to use closed-loop velocity control. Set to true to disable closed-loop.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
   {
-    setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)),
-                       isOpenLoop);
+    desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS);
+
+    // Sets states
+    for (SwerveModule module : swerveModules)
+    {
+      module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false);
+    }
   }
 
   /**
@@ -636,7 +646,7 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
     SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
     SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
 
-    setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
+    setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false);
   }
 
   /**

From 359358e4da4a25b4ba50bf1fc5d146ade5303e07 Mon Sep 17 00:00:00 2001
From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com>
Date: Tue, 8 Oct 2024 15:56:13 -0500
Subject: [PATCH 18/18] Added 5v analog option

---
 src/main/java/swervelib/parser/json/DeviceJson.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java
index 48551e50..bcc269da 100644
--- a/src/main/java/swervelib/parser/json/DeviceJson.java
+++ b/src/main/java/swervelib/parser/json/DeviceJson.java
@@ -74,6 +74,8 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor)
         return new SparkMaxEncoderSwerve(motor, 360);
       case "sparkmax_analog":
         return new SparkMaxAnalogEncoderSwerve(motor, 3.3);
+      case "sparkmax_analog5v":
+        return new SparkMaxAnalogEncoderSwerve(motor, 5);
       case "canandcoder_can":
       case "canandmag_can":
         return new CanAndMagSwerve(id);