Upgrading to 2025.7.0

swervelib/SwerveDrive.java

@@ -6,7 +6,6 @@ import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.KilogramSquareMeters;
 import static edu.wpi.first.units.Units.Kilograms;
 import static edu.wpi.first.units.Units.Meters;
-import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.Newtons;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
@@ -52,14 +51,11 @@ import java.util.Map;
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
-
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
-import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
 import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
 import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
-
 import swervelib.encoders.CANCoderSwerve;
 import swervelib.imu.Pigeon2Swerve;
 import swervelib.imu.SwerveIMU;
@@ -75,7 +71,7 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 /**
  * Swerve Drive class representing and controlling the swerve drive.
  */
-public class SwerveDrive
+public class SwerveDrive implements AutoCloseable
 {
 
   /**
@@ -118,16 +114,20 @@ public class SwerveDrive
    */
   private final DoublePublisher     rawIMUPublisher
                                                                                     = NetworkTableInstance.getDefault()
+                                                                                                          .getTable(
+                                                                                                              "SmartDashboard")
                                                                                                           .getDoubleTopic(
-                                                                                                              "swerve/Raw IMU Yaw")
+                                                                                                              "swerve/imu/raw")
                                                                                                           .publish();
   /**
    * NT4 Publisher for the IMU reading adjusted by offset and inversion.
    */
   private final DoublePublisher     adjustedIMUPublisher
                                                                                     = NetworkTableInstance.getDefault()
+                                                                                                          .getTable(
+                                                                                                              "SmartDashboard")
                                                                                                           .getDoubleTopic(
-                                                                                                              "swerve/Adjusted IMU Yaw")
+                                                                                                              "swerve/imu/adjusted")
                                                                                                           .publish();
   /**
    * Field object.
@@ -224,7 +224,9 @@ public class SwerveDrive
       SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS,
       Pose2d startingPose)
   {
-    this.maxChassisSpeedMPS = maxSpeedMPS;
+    this.attainableMaxTranslationalSpeedMetersPerSecond = this.maxChassisSpeedMPS = maxSpeedMPS;
+    this.attainableMaxRotationalVelocityRadiansPerSecond = Math.PI *
+                                                           2; // Defaulting to something reasonable for most robots
     swerveDriveConfiguration = config;
     swerveController = new SwerveController(controllerConfig);
     // Create Kinematics from swerve module locations.
@@ -247,17 +249,17 @@ public class SwerveDrive
                                                                               .withCustomModuleTranslations(config.moduleLocationsMeters)
                                                                               .withGyro(config.getGyroSim())
                                                                               .withSwerveModule(new SwerveModuleSimulationConfig(
-                                                                                  config.getDriveMotorSim(),
+                                                                                                    config.getDriveMotorSim(),
-                                                                                  config.getAngleMotorSim(),
+                                                                                                    config.getAngleMotorSim(),
-                                                                                  config.physicalCharacteristics.conversionFactor.drive.gearRatio,
+                                                                                                    config.physicalCharacteristics.conversionFactor.drive.gearRatio,
-                                                                                  config.physicalCharacteristics.conversionFactor.angle.gearRatio,
+                                                                                                    config.physicalCharacteristics.conversionFactor.angle.gearRatio,
-                                                                                  Volts.of(config.physicalCharacteristics.driveFrictionVoltage),
+                                                                                                    Volts.of(config.physicalCharacteristics.driveFrictionVoltage),
-                                                                                  Volts.of(config.physicalCharacteristics.angleFrictionVoltage),
+                                                                                                    Volts.of(config.physicalCharacteristics.angleFrictionVoltage),
-                                                                                  Inches.of(
+                                                                                                    Inches.of(
-                                                                                      config.physicalCharacteristics.conversionFactor.drive.diameter /
+                                                                                                        config.physicalCharacteristics.conversionFactor.drive.diameter /
-                                                                                      2),
+                                                                                                        2),
-                                                                                  KilogramSquareMeters.of(0.02),
+                                                                                                    KilogramSquareMeters.of(0.02),
-                                                                                  config.physicalCharacteristics.wheelGripCoefficientOfFriction)
+                                                                                                    config.physicalCharacteristics.wheelGripCoefficientOfFriction)
                                                                                                );
 
       mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose);
@@ -332,6 +334,16 @@ public class SwerveDrive
     HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL);
   }
 
+  @Override
+  public void close() {
+    imu.close();
+    tunerXRecommendation.close();
+
+    for (var module : swerveModules) {
+      module.close();
+    }
+  }
+
   /**
    * Update the cache validity period for the robot.
    *
@@ -567,7 +579,7 @@ public class SwerveDrive
 
     if (fieldRelative)
     {
-      ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
+      velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
     }
     drive(velocity, isOpenLoop, new Translation2d());
   }
@@ -619,31 +631,46 @@ public class SwerveDrive
   }
 
   /**
-   * Set the maximum speeds for desaturation.
+   * Set the maximum attainable speeds for desaturation.
    *
    * @param attainableMaxTranslationalSpeedMetersPerSecond  The absolute max speed that your robot can reach while
    *                                                        translating in meters per second.
    * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in
    *                                                        radians per second.
    */
-  public void setMaximumSpeeds(
+  public void setMaximumAttainableSpeeds(
       double attainableMaxTranslationalSpeedMetersPerSecond,
       double attainableMaxRotationalVelocityRadiansPerSecond)
   {
     this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond;
     this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond;
-    this.swerveController.config.maxAngularVelocity = attainableMaxRotationalVelocityRadiansPerSecond;
+  }
+
+  /**
+   * Set the maximum allowable speeds for desaturation.
+   *
+   * @param maxTranslationalSpeedMetersPerSecond  The allowable max speed that your robot should reach while translating
+   *                                              in meters per second.
+   * @param maxRotationalVelocityRadiansPerSecond The allowable max speed the robot should reach while rotating in
+   *                                              radians per second.
+   */
+  public void setMaximumAllowableSpeeds(
+      double maxTranslationalSpeedMetersPerSecond,
+      double maxRotationalVelocityRadiansPerSecond)
+  {
+    this.maxChassisSpeedMPS = maxTranslationalSpeedMetersPerSecond;
+    this.swerveController.config.maxAngularVelocity = maxRotationalVelocityRadiansPerSecond;
   }
 
   /**
    * Get the maximum velocity from {@link SwerveDrive#attainableMaxTranslationalSpeedMetersPerSecond} or
-   * {@link SwerveDrive#maxChassisSpeedMPS} whichever is higher.
+   * {@link SwerveDrive#maxChassisSpeedMPS} whichever is the lower limit on the robot's speed.
    *
-   * @return Maximum speed in meters/second.
+   * @return Minimum speed in meters/second of physically attainable and user allowable limits.
    */
   public double getMaximumChassisVelocity()
   {
-    return Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS);
+    return Math.min(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS);
   }
 
   /**
@@ -651,9 +678,9 @@ public class SwerveDrive
    *
    * @return {@link LinearVelocity} representing the maximum drive speed of a module.
    */
-  public LinearVelocity getMaximumModuleDriveVelocity()
+  public double getMaximumModuleDriveVelocity()
   {
-    return swerveModules[0].getMaxVelocity();
+    return swerveModules[0].getMaxDriveVelocityMetersPerSecond();
   }
 
   /**
@@ -668,13 +695,13 @@ public class SwerveDrive
 
   /**
    * Get the maximum angular velocity, either {@link SwerveDrive#attainableMaxRotationalVelocityRadiansPerSecond} or
-   * {@link SwerveControllerConfiguration#maxAngularVelocity}.
+   * {@link SwerveControllerConfiguration#maxAngularVelocity}, whichever is the lower limit on the robot's speed.
    *
-   * @return Maximum angular velocity in radians per second.
+   * @return Minimum angular velocity in radians per second of physically attainable and user allowable limits.
    */
   public double getMaximumChassisAngularVelocity()
   {
-    return Math.max(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity);
+    return Math.min(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity);
   }
 
   /**
@@ -688,8 +715,9 @@ public class SwerveDrive
                                   boolean isOpenLoop)
   {
     // Desaturates wheel speeds
-    double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
+    double maxModuleSpeedMPS = getMaximumModuleDriveVelocity();
-    if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
+    if ((attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) &&
+        attainableMaxTranslationalSpeedMetersPerSecond != maxChassisSpeedMPS)
     {
       SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed,
                                                   maxModuleSpeedMPS,
@@ -719,7 +747,7 @@ public class SwerveDrive
   public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
   {
     SwerveDriveTelemetry.startCtrlCycle();
-    double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
+    double maxModuleSpeedMPS = getMaximumModuleDriveVelocity();
     desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
     SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS);
 
@@ -748,6 +776,9 @@ public class SwerveDrive
     }
     for (SwerveModule module : swerveModules)
     {
+      module.applyStateOptimizations(states[module.moduleNumber]);
+      module.applyAntiJitter(states[module.moduleNumber], false);
+      
       // from the module configuration, obtain necessary information to calculate feed-forward
       // Warning: Will not work well if motor is not what we are expecting.
       // Warning: Should replace module.getDriveMotor().simMotor with expected motor type first.
@@ -1128,7 +1159,7 @@ public class SwerveDrive
   {
     SwerveDriveTelemetry.startOdomCycle();
     odometryLock.lock();
-    invalidateCache();
+//    invalidateCache();
     try
     {
       // Update odometry
@@ -1255,7 +1286,7 @@ public class SwerveDrive
    *                                 {@link Timer#getFPGATimestamp()} or similar sources.
    * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the
    *                                 {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement,
-   *                                 for best accuracy calculate the standard deviation at 2 or more  points and fit a
+   *                                 for best accuracy calculate the standard deviation at 2 or more points and fit a
    *                                 line to it 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.
@@ -1355,10 +1386,34 @@ public class SwerveDrive
     }
   }
 
+  /**
+   * Set the motor controller closed loop feedback device to the defined external absolute encoder, with the given
+   * offset from the supplied configuration, overwriting any native offset.
+   */
+  public void useExternalFeedbackSensor()
+  {
+    for (SwerveModule module : swerveModules)
+    {
+      module.useExternalFeedbackSensor();
+    }
+  }
+
+  /**
+   * Set the motor controller closed loop feedback device to the internal encoder instead of the absolute encoder.
+   */
+  public void useInternalFeedbackSensor()
+  {
+    for (SwerveModule module : swerveModules)
+    {
+      module.useInternalFeedbackSensor();
+    }
+  }
+
   /**
    * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
    * internal offsets to prevent double offsetting.
    */
+  @Deprecated
   public void pushOffsetsToEncoders()
   {
     for (SwerveModule module : swerveModules)
@@ -1370,6 +1425,7 @@ public class SwerveDrive
   /**
    * Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0
    */
+  @Deprecated
   public void restoreInternalOffset()
   {
     for (SwerveModule module : swerveModules)
@@ -1378,6 +1434,20 @@ public class SwerveDrive
     }
   }
 
+  /**
+   * Set module optimization to be utilized or not. Sometimes it is desirable to be enabled for debugging purposes
+   * only.
+   *
+   * @param enabled Optimization enabled state.
+   */
+  public void setModuleStateOptimization(boolean enabled)
+  {
+    for (SwerveModule module : swerveModules)
+    {
+      module.setModuleStateOptimization(enabled);
+    }
+  }
+
   /**
    * Enable auto-centering module wheels. This has a side effect of causing some jitter to the robot when a PID is not
    * tuned perfectly. This function is a wrapper for {@link SwerveModule#setAntiJitter(boolean)} to perform

swervelib/SwerveDriveTest.java

@@ -9,6 +9,8 @@ import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.units.measure.MutAngle;
 import edu.wpi.first.units.measure.MutAngularVelocity;
 import edu.wpi.first.units.measure.MutDistance;
@@ -27,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.telemetry.SwerveDriveTelemetry;
 
 /**
  * Class to perform tests on the swerve drive.
@@ -101,7 +104,7 @@ public class SwerveDriveTest
    * Power the drive motors for the swerve drive to a set voltage.
    *
    * @param swerveDrive {@link SwerveDrive} to control.
-   * @param volts       DutyCycle percentage of voltage to send to drive motors.
+   * @param volts       Voltage to send to drive motors.
    */
   public static void powerDriveMotorsVoltage(SwerveDrive swerveDrive, double volts)
   {
@@ -135,6 +138,60 @@ public class SwerveDriveTest
     angleModules(swerveDrive, Rotation2d.fromDegrees(0));
   }
 
+  /**
+   * Set the modules to their rotary position to allow running sysid and spinning the robot
+   *
+   * @param swerveDrive Swerve Drive to control.
+   */
+  public static void setModulesToRotaryPosition(SwerveDrive swerveDrive)
+  {
+    SwerveModuleState[] rotaryStates = swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1));
+    for (int i = 0; i < swerveDrive.getModules().length; i++)
+    {
+      swerveDrive.getModules()[i].getAngleMotor().setReference(rotaryStates[i].angle.getDegrees(), 0);
+    }
+  }
+
+  /**
+   * Set the sim modules to center to 0 and power them to drive in a voltage. Calling this function in sim is equivalent
+   * to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real
+   * robot.
+   *
+   * @param swerveDrive      {@link SwerveDrive} to control.
+   * @param volts            Voltage to send to drive motors.
+   * @param testWithSpinning - Whether to make the robot spin in place instead of driving in a straight line, true to
+   *                         make the robot spin, false to make the robot drive in straight line
+   */
+  public static void runDriveMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts,
+                                                                boolean testWithSpinning)
+  {
+    SwerveModuleState[] rotaryStates = swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1));
+    for (int i = 0; i < swerveDrive.getModules().length; i++)
+    {
+      swerveDrive.getModules()[i].getSimModule().runDriveMotorCharacterization(
+          testWithSpinning
+          ? rotaryStates[i].angle
+          : Rotation2d.kZero,
+          volts);
+    }
+  }
+
+  /**
+   * Set the sim modules to center to 0 and power them to drive in a voltage. Calling this function in sim is equivalent
+   * to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real
+   * robot.
+   *
+   * @param swerveDrive {@link SwerveDrive} to control.
+   * @param volts       Voltage to send to angle motors.
+   */
+  public static void runAngleMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts)
+  {
+    for (SwerveModule module : swerveDrive.getModules())
+    {
+      module.getSimModule().runAngleMotorCharacterization(volts);
+    }
+  }
+
   /**
    * Find the minimum amount of power required to move the swerve drive motors.
    *
@@ -299,19 +356,35 @@ public class SwerveDriveTest
   /**
    * Sets up the SysId runner and logger for the drive motors
    *
-   * @param config          - The SysIdRoutine.Config to use
+   * @param config           - The SysIdRoutine.Config to use
-   * @param swerveSubsystem - the subsystem to add to requirements
+   * @param swerveSubsystem  - the subsystem to add to requirements
-   * @param swerveDrive     - the SwerveDrive from which to access motor info
+   * @param swerveDrive      - the SwerveDrive from which to access motor info
-   * @param maxVolts        - The maximum voltage that should be applied to the drive motors.
+   * @param maxVolts         - The maximum voltage that should be applied to the drive motors.
+   * @param testWithSpinning - Whether to make the robot spin in place instead of driving in a straight line, true to
+   *                         make the robot spin, false to make the robot drive in straight line
    * @return A SysIdRoutine runner
    */
   public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swerveSubsystem,
-                                                  SwerveDrive swerveDrive, double maxVolts)
+                                                  SwerveDrive swerveDrive, double maxVolts, boolean testWithSpinning)
   {
     return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
         (Voltage voltage) -> {
-          SwerveDriveTest.centerModules(swerveDrive);
+          if (!SwerveDriveTelemetry.isSimulation)
-          SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
+          {
+            if (testWithSpinning)
+            {
+              SwerveDriveTest.setModulesToRotaryPosition(swerveDrive);
+            } else
+            {
+              SwerveDriveTest.centerModules(swerveDrive);
+            }
+            SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
+          } else
+          {
+            SwerveDriveTest.runDriveMotorsCharacterizationOnSimModules(swerveDrive,
+                                                                       voltage.in(Volts),
+                                                                       testWithSpinning);
+          }
         },
         log -> {
           for (SwerveModule module : swerveDrive.getModules())
@@ -382,8 +455,14 @@ public class SwerveDriveTest
   {
     return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
         (Voltage voltage) -> {
-          SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
+          if (!SwerveDriveTelemetry.isSimulation)
-          SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
+          {
+            SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
+            SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
+          } else
+          {
+            SwerveDriveTest.runAngleMotorsCharacterizationOnSimModules(swerveDrive, voltage.in(Volts));
+          }
         },
         log -> {
           for (SwerveModule module : swerveDrive.getModules())

swervelib/SwerveInputStream.java

@@ -1,6 +1,7 @@
 package swervelib;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -44,83 +45,111 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
   /**
    * Translation suppliers.
    */
-  private final DoubleSupplier            controllerTranslationX;
+  private final DoubleSupplier                  controllerTranslationX;
   /**
    * Translational supplier.
    */
-  private final DoubleSupplier            controllerTranslationY;
+  private final DoubleSupplier                  controllerTranslationY;
   /**
    * {@link SwerveDrive} object for transformations.
    */
-  private final SwerveDrive               swerveDrive;
+  private final SwerveDrive                     swerveDrive;
   /**
    * Rotation supplier as angular velocity.
    */
-  private Optional<DoubleSupplier>  controllerOmega      = Optional.empty();
+  private       Optional<DoubleSupplier>        controllerOmega               = Optional.empty();
   /**
    * Controller supplier as heading.
    */
-  private       Optional<DoubleSupplier>  controllerHeadingX     = Optional.empty();
+  private       Optional<DoubleSupplier>        controllerHeadingX            = Optional.empty();
   /**
    * Controller supplier as heading.
    */
-  private       Optional<DoubleSupplier>  controllerHeadingY     = Optional.empty();
+  private       Optional<DoubleSupplier>        controllerHeadingY            = Optional.empty();
   /**
    * Axis deadband for the controller.
    */
-  private Optional<Double>          axisDeadband         = Optional.empty();
+  private       Optional<Double>                axisDeadband                  = Optional.empty();
   /**
    * Translational axis scalar value, should be between (0, 1].
    */
-  private Optional<Double>          translationAxisScale = Optional.empty();
+  private       Optional<Double>                translationAxisScale          = Optional.empty();
   /**
    * Angular velocity axis scalar value, should be between (0, 1]
    */
-  private Optional<Double>          omegaAxisScale       = Optional.empty();
+  private       Optional<Double>                omegaAxisScale                = Optional.empty();
   /**
    * Target to aim at.
    */
-  private Optional<Pose2d>          aimTarget            = Optional.empty();
+  private       Optional<Pose2d>                aimTarget                     = Optional.empty();
+  /**
+   * Target {@link Supplier<Pose2d>} to drive towards when driveToPose is enabled.
+   */
+  private       Optional<Supplier<Pose2d>>      driveToPose                   = Optional.empty();
+  /**
+   * {@link ProfiledPIDController} for the X translation while driving to a pose. Units are m/s
+   */
+  private       Optional<ProfiledPIDController> driveToPoseXPIDController     = Optional.empty();
+  /**
+   * {@link ProfiledPIDController} for the Y translation while driving to a pose. Units are m/s
+   */
+  private       Optional<ProfiledPIDController> driveToPoseYPIDController     = Optional.empty();
+  /**
+   * {@link ProfiledPIDController} for the Rotational axis while driving to a pose. Units are m/s
+   */
+  private       Optional<ProfiledPIDController> driveToPoseOmegaPIDController = Optional.empty();
   /**
    * Output {@link ChassisSpeeds} based on heading while this is True.
    */
-  private       Optional<BooleanSupplier> headingEnabled         = Optional.empty();
+  private       Optional<BooleanSupplier>       headingEnabled                = Optional.empty();
   /**
    * Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY}
    */
-  private       Optional<Rotation2d>      lockedHeading          = Optional.empty();
+  private       Optional<Rotation2d>            lockedHeading                 = Optional.empty();
   /**
    * Output {@link ChassisSpeeds} based on aim while this is True.
    */
-  private       Optional<BooleanSupplier> aimEnabled             = Optional.empty();
+  private       Optional<BooleanSupplier>       aimEnabled                    = Optional.empty();
+  /**
+   * Output {@link ChassisSpeeds} to move to a specific {@link Pose2d}.
+   */
+  private       Optional<BooleanSupplier>       driveToPoseEnabled            = Optional.empty();
   /**
    * Maintain current heading and drive without rotating, ideally.
    */
-  private       Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
+  private       Optional<BooleanSupplier>       translationOnlyEnabled        = Optional.empty();
   /**
    * Cube the translation magnitude from the controller.
    */
-  private Optional<BooleanSupplier> translationCube      = Optional.empty();
+  private       Optional<BooleanSupplier>       translationCube               = Optional.empty();
   /**
    * Cube the angular velocity axis from the controller.
    */
-  private Optional<BooleanSupplier> omegaCube            = Optional.empty();
+  private       Optional<BooleanSupplier>       omegaCube                     = Optional.empty();
   /**
    * Robot relative oriented output expected.
    */
-  private Optional<BooleanSupplier> robotRelative        = Optional.empty();
+  private       Optional<BooleanSupplier>       robotRelative                 = Optional.empty();
   /**
    * Field oriented chassis output is relative to your current alliance.
    */
-  private Optional<BooleanSupplier> allianceRelative     = Optional.empty();
+  private       Optional<BooleanSupplier>       allianceRelative              = Optional.empty();
+  /**
+   * Heading offset enable state.
+   */
+  private       Optional<BooleanSupplier>       headingOffsetEnabled          = Optional.empty();
+  /**
+   * Heading offset to apply during heading based control.
+   */
+  private       Optional<Rotation2d>            headingOffset                 = Optional.empty();
   /**
    * {@link SwerveController} for simple control over heading.
    */
-  private       SwerveController          swerveController       = null;
+  private       SwerveController                swerveController              = null;
   /**
    * Current {@link SwerveInputMode} to use.
    */
-  private       SwerveInputMode           currentMode            = SwerveInputMode.ANGULAR_VELOCITY;
+  private       SwerveInputMode                 currentMode                   = SwerveInputMode.ANGULAR_VELOCITY;
 
 
   /**
@@ -195,9 +224,14 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     newStream.axisDeadband = axisDeadband;
     newStream.translationAxisScale = translationAxisScale;
     newStream.omegaAxisScale = omegaAxisScale;
+    newStream.driveToPose = driveToPose;
+    newStream.driveToPoseXPIDController = driveToPoseXPIDController;
+    newStream.driveToPoseYPIDController = driveToPoseYPIDController;
+    newStream.driveToPoseOmegaPIDController = driveToPoseOmegaPIDController;
     newStream.aimTarget = aimTarget;
     newStream.headingEnabled = headingEnabled;
     newStream.aimEnabled = aimEnabled;
+    newStream.driveToPoseEnabled = driveToPoseEnabled;
     newStream.currentMode = currentMode;
     newStream.translationOnlyEnabled = translationOnlyEnabled;
     newStream.lockedHeading = lockedHeading;
@@ -206,6 +240,8 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     newStream.translationCube = translationCube;
     newStream.robotRelative = robotRelative;
     newStream.allianceRelative = allianceRelative;
+    newStream.headingOffsetEnabled = headingOffsetEnabled;
+    newStream.headingOffset = headingOffset;
     return newStream;
   }
 
@@ -233,6 +269,103 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     return this;
   }
 
+  /**
+   * Drive to a given pose with the provided {@link ProfiledPIDController}s
+   *
+   * @param pose               {@link Supplier<Pose2d>} for ease of use.
+   * @param xPIDController     PID controller for the X axis, units are m/s.
+   * @param yPIDController     PID controller for the Y axis, units are m/s.
+   * @param omegaPIDController PID Controller for rotational axis, units are rad/s.
+   * @return self
+   */
+  public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController xPIDController,
+                                       ProfiledPIDController yPIDController, ProfiledPIDController omegaPIDController)
+  {
+    driveToPose = Optional.of(pose);
+    driveToPoseXPIDController = Optional.of(xPIDController);
+    driveToPoseYPIDController = Optional.of(yPIDController);
+    driveToPoseOmegaPIDController = Optional.of(omegaPIDController);
+    return this;
+  }
+
+  /**
+   * Drive to a given pose with the provided {@link ProfiledPIDController}s
+   *
+   * @param pose        {@link Supplier<Pose2d>} for ease of use.
+   * @param translation PID controller for the X and Y axis, units are m/s.
+   * @param rotation    PID Controller for rotational axis, units are rad/s.
+   * @return self
+   */
+  public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController translation,
+                                       ProfiledPIDController rotation)
+  {
+    return driveToPose(pose, translation, new ProfiledPIDController(translation.getP(),
+                                                                    translation.getI(),
+                                                                    translation.getD(),
+                                                                    translation.getConstraints()), rotation);
+  }
+
+  /**
+   * Enable driving to the target pose.
+   *
+   * @param enabled Enable state of drive to pose.
+   * @return self.
+   */
+  public SwerveInputStream driveToPoseEnabled(BooleanSupplier enabled)
+  {
+    driveToPoseEnabled = Optional.of(enabled);
+    return this;
+  }
+
+  /**
+   * Enable driving to the target pose.
+   *
+   * @param enabled Enable state of drive to pose.
+   * @return self.
+   */
+  public SwerveInputStream driveToPoseEnabled(boolean enabled)
+  {
+    driveToPoseEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
+    return this;
+  }
+
+
+  /**
+   * Heading offset enabled boolean supplier.
+   *
+   * @param enabled Enable state
+   * @return self
+   */
+  public SwerveInputStream headingOffset(BooleanSupplier enabled)
+  {
+    headingOffsetEnabled = Optional.of(enabled);
+    return this;
+  }
+
+  /**
+   * Heading offset enable
+   *
+   * @param enabled Enable state
+   * @return self
+   */
+  public SwerveInputStream headingOffset(boolean enabled)
+  {
+    headingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
+    return this;
+  }
+
+  /**
+   * Set the heading offset angle.
+   *
+   * @param angle {@link Rotation2d} offset to apply
+   * @return self
+   */
+  public SwerveInputStream headingOffset(Rotation2d angle)
+  {
+    headingOffset = Optional.of(angle);
+    return this;
+  }
+
   /**
    * Modify the output {@link ChassisSpeeds} so that it is always relative to your alliance.
    *
@@ -476,7 +609,19 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
    */
   private SwerveInputMode findMode()
   {
-    if (translationOnlyEnabled.isPresent() && translationOnlyEnabled.get().getAsBoolean())
+    if (driveToPoseEnabled.isPresent() && driveToPoseEnabled.get().getAsBoolean())
+    {
+      if (driveToPose.isPresent())
+      {
+        if (driveToPoseOmegaPIDController.isPresent() && driveToPoseXPIDController.isPresent() &&
+            driveToPoseYPIDController.isPresent())
+        {
+          return SwerveInputMode.DRIVE_TO_POSE;
+        }
+        DriverStation.reportError("Drive to pose not supplied with pid controllers.", false);
+      }
+      DriverStation.reportError("Drive to pose enabled without supplier present.", false);
+    } else if (translationOnlyEnabled.isPresent() && translationOnlyEnabled.get().getAsBoolean())
     {
       return SwerveInputMode.TRANSLATION_ONLY;
     } else if (aimEnabled.isPresent() && aimEnabled.get().getAsBoolean())
@@ -526,17 +671,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         lockedHeading = Optional.empty();
         break;
       }
-      case ANGULAR_VELOCITY ->
+      case ANGULAR_VELOCITY, HEADING, AIM, DRIVE_TO_POSE ->
-      {
-        // Do nothing
-        break;
-      }
-      case HEADING ->
-      {
-        // Do nothing
-        break;
-      }
-      case AIM ->
       {
         // Do nothing
         break;
@@ -551,7 +686,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         lockedHeading = Optional.of(swerveDrive.getOdometryHeading());
         break;
       }
-      case ANGULAR_VELOCITY ->
+      case ANGULAR_VELOCITY, DRIVE_TO_POSE ->
       {
         if (swerveDrive.headingCorrection)
         {
@@ -559,12 +694,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         }
         break;
       }
-      case HEADING ->
+      case HEADING, AIM ->
-      {
-        // Do nothing
-        break;
-      }
-      case AIM ->
       {
         // Do nothing
         break;
@@ -651,16 +781,16 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
   }
 
   /**
-   * Change {@link ChassisSpeeds} to robot relative.
+   * Change {@link ChassisSpeeds} from robot relative if enabled.
    *
-   * @param fieldRelativeSpeeds Field relative speeds to translate into robot-relative speeds.
+   * @param fieldRelativeSpeeds Field or robot relative speeds to translate into robot-relative speeds.
-   * @return Robot relative {@link ChassisSpeeds}.
+   * @return Field relative {@link ChassisSpeeds}.
    */
   private ChassisSpeeds applyRobotRelativeTranslation(ChassisSpeeds fieldRelativeSpeeds)
   {
     if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
     {
-      return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, swerveDrive.getOdometryHeading());
+      return ChassisSpeeds.fromRobotRelativeSpeeds(fieldRelativeSpeeds, swerveDrive.getOdometryHeading());
     }
     return fieldRelativeSpeeds;
   }
@@ -709,6 +839,24 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     return fieldRelativeRotation;
   }
 
+  /**
+   * Adds offset to rotation if one is set.
+   *
+   * @param fieldRelativeRotation Field-relative {@link Rotation2d} to offset
+   * @return Offsetted {@link Rotation2d}
+   */
+  private Rotation2d applyHeadingOffset(Rotation2d fieldRelativeRotation)
+  {
+    if (headingOffsetEnabled.isPresent() && headingOffsetEnabled.get().getAsBoolean())
+    {
+      if (headingOffset.isPresent())
+      {
+        return fieldRelativeRotation.rotateBy(headingOffset.get());
+      }
+    }
+    return fieldRelativeRotation;
+  }
+
   /**
    * Gets a {@link ChassisSpeeds}
    *
@@ -759,12 +907,22 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
       case HEADING ->
       {
         omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
-                                                                  applyAllianceAwareRotation(Rotation2d.fromRadians(
+                                                                  applyHeadingOffset(
-                                                                      swerveController.getJoystickAngle(
+                                                                      applyAllianceAwareRotation(
-                                                                          controllerHeadingX.get()
+                                                                          Rotation2d.fromRadians(
-                                                                                            .getAsDouble(),
+                                                                              swerveController.getJoystickAngle(
-                                                                          controllerHeadingY.get()
+                                                                                  controllerHeadingX.get()
-                                                                                            .getAsDouble()))).getRadians());
+                                                                                                    .getAsDouble(),
+                                                                                  controllerHeadingY.get()
+                                                                                                    .getAsDouble())))).getRadians());
+
+        // Prevent rotation if controller heading inputs are not past axisDeadband
+        if (Math.abs(controllerHeadingX.get().getAsDouble()) + Math.abs(controllerHeadingY.get().getAsDouble()) <
+            axisDeadband.get())
+        {
+          omegaRadiansPerSecond = 0;
+        }
+
         speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
         break;
       }
@@ -777,6 +935,17 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
         break;
       }
+      case DRIVE_TO_POSE ->
+      {
+        Pose2d target = driveToPose.get().get();
+        Pose2d pose   = swerveDrive.getPose();
+        omegaRadiansPerSecond = driveToPoseOmegaPIDController.get().calculate(pose.getRotation()
+                                                                                  .getRadians(),
+                                                                              target.getRotation().getRadians());
+        speeds = new ChassisSpeeds(driveToPoseXPIDController.get().calculate(pose.getX(), target.getX()),
+                                   driveToPoseYPIDController.get().calculate(pose.getY(), target.getY()),
+                                   omegaRadiansPerSecond);
+      }
     }
 
     currentMode = newMode;
@@ -804,6 +973,10 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     /**
      * Output based off of targeting.
      */
-    AIM
+    AIM,
+    /**
+     * Drive to a target pose.
+     */
+    DRIVE_TO_POSE
   }
 }

swervelib/SwerveModule.java

@@ -1,5 +1,6 @@
 package swervelib;
 
+import static edu.wpi.first.units.Units.InchesPerSecond;
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
@@ -33,7 +34,7 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 /**
  * The Swerve Module class which represents and controls Swerve Modules for the swerve drive.
  */
-public class SwerveModule
+public class SwerveModule implements AutoCloseable
 {
 
   /**
@@ -72,6 +73,29 @@ public class SwerveModule
    * An {@link Alert} for if there is no Absolute Encoder on the module.
    */
   private final Alert                  noEncoderWarning;
+  /**
+   * An {@link Alert} for if there is no Absolute Encoder on the module.
+   */
+  private final Alert            externalSensorIsNull         = new Alert("No absolute Encoder found.",
+                                                                          AlertType.kError);
+  /**
+   * An {@link Alert} for if the offset is 0 degrees.
+   */
+  private final Alert            internalOffsetIsZero         = new Alert(
+      "Absolute encoder offset is 0, this may be a problem.",
+      AlertType.kWarning);
+  /**
+   * An {@link Alert} for if the angle/steer/azimuth motor is incompatible with the absolute encoder.
+   */
+  private final Alert            externalFeedbackIncompatible = new Alert(
+      "Absolute encoder is incompatible, cannot set as an external feedback device.",
+      AlertType.kError);
+  /**
+   * An {@link Alert} for if the absolute encoder cannot set an offset.
+   */
+  private final Alert            externalOffsetIncompatible   = new Alert(
+      "Absolute encoder is incompatible, cannot set an offset internally.",
+      AlertType.kError);
   /**
    * NT4 Raw Absolute Angle publisher for the absolute encoder.
    */
@@ -108,6 +132,10 @@ public class SwerveModule
    * Maximum {@link LinearVelocity} for the drive motor of the swerve module.
    */
   private       LinearVelocity         maxDriveVelocity;
+  /**
+   * Maximum velocity for the drive motor of the swerve module.
+   */
+  private       double           maxDriveVelocityMetersPerSecond;
   /**
    * Maximum {@link AngularVelocity} for the azimuth/angle motor of the swerve module.
    */
@@ -119,7 +147,7 @@ public class SwerveModule
   /**
    * Anti-Jitter AKA auto-centering disabled.
    */
-  private       boolean                antiJitterEnabled          = true;
+  private       boolean          antiJitterEnabled            = true;
   /**
    * Last swerve module state applied.
    */
@@ -132,18 +160,22 @@ public class SwerveModule
    * Simulated swerve module.
    */
   private       SwerveModuleSimulation simModule;
+  /**
+   * Enables utilization off {@link SwerveModuleState#optimize(Rotation2d)}
+   */
+  private       boolean          optimizeSwerveModuleState    = true;
   /**
    * Encoder synchronization queued.
    */
-  private       boolean                synchronizeEncoderQueued   = false;
+  private       boolean          synchronizeEncoderQueued     = false;
   /**
    * Encoder, Absolute encoder synchronization enabled.
    */
-  private       boolean                synchronizeEncoderEnabled  = false;
+  private       boolean          synchronizeEncoderEnabled    = false;
   /**
    * Encoder synchronization deadband in degrees.
    */
-  private       double                 synchronizeEncoderDeadband = 3;
+  private       double           synchronizeEncoderDeadband   = 3;
 
 
   /**
@@ -196,7 +228,10 @@ public class SwerveModule
     absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20);
 
     // Config angle motor/controller
-    angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
+    if (!angleMotor.usingExternalFeedbackSensor())
+    {
+      angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
+    }
     angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
     angleMotor.configurePIDWrapping(0, 360);
     angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
@@ -255,6 +290,14 @@ public class SwerveModule
         "swerve/modules/" + configuration.name + "/Angle Setpoint").publish();
   }
 
+  @Override
+  public void close()
+  {
+    angleMotor.close();
+    driveMotor.close();
+    absoluteEncoder.close();
+  }
+
   /**
    * Get the default {@link SimpleMotorFeedforward} for the swerve module drive motor.
    *
@@ -269,6 +312,31 @@ public class SwerveModule
                                              configuration.physicalCharacteristics.wheelGripCoefficientOfFriction);
   }
 
+  /**
+   * Set utilization of {@link SwerveModuleState#optimize(Rotation2d)} which should be disabled for some debugging.
+   *
+   * @param optimizationState Optimization enabled.
+   */
+  public void setModuleStateOptimization(boolean optimizationState)
+  {
+    optimizeSwerveModuleState = optimizationState;
+    if (!optimizeSwerveModuleState)
+    {
+      angleMotor.disablePIDWrapping();
+      angleMotor.burnFlash();
+    }
+  }
+
+  /**
+   * Check if the module state optimization used by {@link SwerveModuleState#optimize(Rotation2d)} is enabled.
+   *
+   * @return optimization state.
+   */
+  public boolean getModuleStateOptimization()
+  {
+    return optimizeSwerveModuleState;
+  }
+
   /**
    * Set the voltage compensation for the swerve module motor.
    *
@@ -406,27 +474,20 @@ public class SwerveModule
    */
   public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
   {
-
+    applyStateOptimizations(desiredState);
-    desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
+    applyAntiJitter(desiredState, force);
-
-    // If we are forcing the angle
-    if (!force && antiJitterEnabled)
-    {
-      // Prevents module rotation if speed is less than 1%
-      SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocity.in(MetersPerSecond), 4));
-    }
 
     // Cosine compensation.
-    LinearVelocity nextVelocity = configuration.useCosineCompensator
+    double nextVelocityMetersPerSecond = configuration.useCosineCompensator
-                                  ? getCosineCompensatedVelocity(desiredState)
+                                         ? getCosineCompensatedVelocity(desiredState)
-                                  : MetersPerSecond.of(desiredState.speedMetersPerSecond);
+                                         : desiredState.speedMetersPerSecond;
-    LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
+    double curVelocityMetersPerSecond = lastState.speedMetersPerSecond;
-    desiredState.speedMetersPerSecond = nextVelocity.magnitude();
+    desiredState.speedMetersPerSecond = nextVelocityMetersPerSecond;
 
     setDesiredState(desiredState,
                     isOpenLoop,
-                    driveMotorFeedforward.calculateWithVelocities(curVelocity.in(MetersPerSecond),
+                    driveMotorFeedforward.calculateWithVelocities(curVelocityMetersPerSecond,
-                                                                  nextVelocity.in(MetersPerSecond)));
+                                                                  nextVelocityMetersPerSecond));
   }
 
   /**
@@ -440,7 +501,6 @@ public class SwerveModule
   public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
                               double driveFeedforwardVoltage)
   {
-
     if (isOpenLoop)
     {
       double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond);
@@ -497,7 +557,7 @@ public class SwerveModule
    * @param desiredState Desired {@link SwerveModuleState} to use.
    * @return Cosine compensated velocity in meters/second.
    */
-  private LinearVelocity getCosineCompensatedVelocity(SwerveModuleState desiredState)
+  private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
   {
     double cosineScalar = 1.0;
     // Taken from the CTRE SwerveModule class.
@@ -515,7 +575,39 @@ public class SwerveModule
       cosineScalar = 1;
     }
 
-    return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar);
+    return desiredState.speedMetersPerSecond * cosineScalar;
+  }
+
+  /**
+   * Apply the {@link SwerveModuleState#optimize(Rotation2d)} function if the module state optimization is enabled while
+   * debugging.
+   *
+   * @param desiredState The desired state to apply the optimization to.
+   */
+  public void applyStateOptimizations(SwerveModuleState desiredState)
+  {
+    // SwerveModuleState optimization might be desired to be disabled while debugging.
+    if (optimizeSwerveModuleState)
+    {
+      desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
+    }
+  }
+
+  /**
+   * Apply anti-jitter to the desired state. This will prevent the module from rotating if the speed requested is too
+   * low. If force is true, the anti-jitter will not be applied.
+   *
+   * @param desiredState The desired state to apply the anti-jitter to.
+   * @param force        Whether to ignore the {@link SwerveModule#antiJitterEnabled} state and apply the anti-jitter
+   *                     anyway.
+   */
+  public void applyAntiJitter(SwerveModuleState desiredState, boolean force)
+  {
+    if (!force && antiJitterEnabled)
+    {
+      // Prevents module rotation if speed is less than 1%
+      SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocityMetersPerSecond, 4));
+    }
   }
 
   /**
@@ -605,10 +697,13 @@ public class SwerveModule
     {
       angle = getRelativePosition();
     }
-    angle %= 360;
+    if (optimizeSwerveModuleState)
-    if (angle < 0.0)
     {
-      angle += 360;
+      angle %= 360;
+      if (angle < 0.0)
+      {
+        angle += 360;
+      }
     }
 
     return angle;
@@ -696,9 +791,64 @@ public class SwerveModule
     return configuration;
   }
 
+
+  /**
+   * Use external sensors for the feedback of the angle/azimuth/steer controller.
+   */
+  public void useExternalFeedbackSensor()
+  {
+    if (absoluteEncoder == null)
+    {
+      externalSensorIsNull.set(true);
+      return;
+    }
+    if (angleOffset == 0)
+    {
+      internalOffsetIsZero.set(true);
+    }
+    if (absoluteEncoder.setAbsoluteEncoderOffset(configuration.angleOffset))
+    {
+      angleMotor.setAbsoluteEncoder(absoluteEncoder);
+      if (angleMotor.usingExternalFeedbackSensor())
+      {
+        angleOffset = 0;
+      } else
+      {
+        externalFeedbackIncompatible.set(true);
+        angleMotor.setAbsoluteEncoder(null);
+        absoluteEncoder.setAbsoluteEncoderOffset(0);
+      }
+
+    } else
+    {
+      externalOffsetIncompatible.set(true);
+      absoluteEncoder.setAbsoluteEncoderOffset(0);
+    }
+  }
+
+  /**
+   * Use external sensors for the feedback of the angle/azimuth/steer controller.
+   */
+  public void useInternalFeedbackSensor()
+  {
+    if (absoluteEncoder == null)
+    {
+      externalSensorIsNull.set(true);
+      return;
+    }
+    if (angleOffset == 0)
+    {
+      internalOffsetIsZero.set(true);
+    }
+    angleMotor.setAbsoluteEncoder(null);
+    absoluteEncoder.setAbsoluteEncoderOffset(0);
+    angleOffset = configuration.angleOffset;
+  }
+
   /**
    * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset.
    */
+  @Deprecated
   public void pushOffsetsToEncoders()
   {
     if (absoluteEncoder != null && angleOffset == configuration.angleOffset)
@@ -759,15 +909,27 @@ public class SwerveModule
    * @return {@link LinearVelocity} max velocity of the drive wheel.
    */
   public LinearVelocity getMaxVelocity()
+  {
+    getMaxDriveVelocityMetersPerSecond();
+    return maxDriveVelocity;
+  }
+
+  /**
+   * Get the maximum drive velocity of the module in Meters Per Second.
+   *
+   * @return Maximum drive motor velocity in Meters Per Second.
+   */
+  public double getMaxDriveVelocityMetersPerSecond()
   {
     if (maxDriveVelocity == null)
     {
-      maxDriveVelocity = MetersPerSecond.of(
+      maxDriveVelocity = InchesPerSecond.of(
-          (RadiansPerSecond.of(driveMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
+          (driveMotor.getSimMotor().freeSpeedRadPerSec /
            configuration.conversionFactors.drive.gearRatio) *
-          configuration.conversionFactors.drive.diameter);
+          configuration.conversionFactors.drive.diameter / 2.0);
+      maxDriveVelocityMetersPerSecond = maxDriveVelocity.in(MetersPerSecond);
     }
-    return maxDriveVelocity;
+    return maxDriveVelocityMetersPerSecond;
   }
 
   /**
@@ -780,7 +942,7 @@ public class SwerveModule
     if (maxAngularVelocity == null)
     {
       maxAngularVelocity = RotationsPerSecond.of(
-          RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) *
+          RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
           configuration.conversionFactors.angle.gearRatio);
     }
     return maxAngularVelocity;
@@ -795,11 +957,26 @@ public class SwerveModule
     {
       rawAbsoluteAnglePublisher.set(absoluteEncoder.getAbsolutePosition());
     }
-    rawAnglePublisher.set(angleMotor.getPosition());
+    if (SwerveDriveTelemetry.isSimulation && SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
-    rawDriveEncoderPublisher.set(drivePositionCache.getValue());
+    {
-    rawDriveVelocityPublisher.set(driveVelocityCache.getValue());
+      SwerveModulePosition pos   = simModule.getPosition();
+      SwerveModuleState    state = simModule.getState();
+      rawAnglePublisher.set(pos.angle.getDegrees());
+      rawDriveEncoderPublisher.set(pos.distanceMeters);
+      rawDriveVelocityPublisher.set(state.speedMetersPerSecond);
+      // For code coverage
+      angleMotor.getPosition();
+      drivePositionCache.getValue();
+      driveVelocityCache.getValue();
+    } else
+    {
+      rawAnglePublisher.set(angleMotor.getPosition());
+      rawDriveEncoderPublisher.set(drivePositionCache.getValue());
+      rawDriveVelocityPublisher.set(driveVelocityCache.getValue());
+    }
     adjAbsoluteAnglePublisher.set(getAbsolutePosition());
     absoluteEncoderIssuePublisher.set(getAbsoluteEncoderReadIssue());
+
   }
 
   /**

swervelib/encoders/AnalogAbsoluteEncoderSwerve.java

@@ -47,6 +47,12 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
         AlertType.kWarning);
   }
 
+  @Override
+  public void close()
+  {
+    encoder.close();
+  }
+
   /**
    * Construct the Encoder given the analog input channel.
    *

swervelib/encoders/CANCoderSwerve.java

@@ -1,6 +1,5 @@
 package swervelib.encoders;
 
-import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.DegreesPerSecond;
 import static edu.wpi.first.units.Units.Milliseconds;
 import static edu.wpi.first.units.Units.Rotations;
@@ -13,7 +12,6 @@ import com.ctre.phoenix6.configs.CANcoderConfigurator;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.signals.MagnetHealthValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.Alert;
@@ -28,7 +26,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
   /**
    * Wait time for status frames to show up.
    */
-  public static double                          STATUS_TIMEOUT_SECONDS = Milliseconds.of(10).in(Seconds);
+  public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(1).in(Seconds);
   /**
    * An {@link Alert} for if the CANCoder magnet field is less than ideal.
    */
@@ -114,6 +112,12 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
         AlertType.kWarning);
   }
 
+  @Override
+  public void close()
+  {
+    encoder.close();
+  }
+
   /**
    * Reset the encoder to factory defaults.
    */
@@ -158,6 +162,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
   {
     readingError = false;
     MagnetHealthValue strength = magnetHealth.refresh().getValue();
+    angle.refresh();
 
     magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green);
     if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
@@ -170,8 +175,6 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
       readingFaulty.set(false);
     }
 
-    angle.refresh();
-
     // Taken from democat's library.
     // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
     for (int i = 0; i < maximumRetries; i++)
@@ -190,8 +193,8 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
     {
       readingIgnored.set(false);
     }
-
+    // Convert from Rotations to Degrees.
-    return angle.getValue().in(Degrees);
+    return angle.getValueAsDouble() * 360;
   }
 
   /**

swervelib/encoders/CanAndMagSwerve.java

@@ -26,7 +26,13 @@ public class CanAndMagSwerve extends SwerveAbsoluteEncoder
   public CanAndMagSwerve(int canid)
   {
     encoder = new Canandmag(canid);
-    settings = encoder.getSettings();
+    settings = new CanandmagSettings();
+  }
+
+  @Override
+  public void close()
+  {
+    encoder.close();
   }
 
   /**

swervelib/encoders/DIODutyCycleEncoderSwerve.java

@@ -0,0 +1,129 @@
+package swervelib.encoders;
+
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * DutyCycle encoders such as "US Digital MA3 with DIO Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
+ * Encoder." attached via a DIO lane.
+ * <p>
+ * Credits to
+ * <a href="https://github.com/p2reneker25/2035-YAGSL/blob/main/swervelib/encoders/DIODutyCycleEncoderSwerve.java">
+ * p2reneker25</a> for building this.
+ */
+public class DIODutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
+{
+
+  /**
+   * Duty Cycle Encoder.
+   */
+  private final DutyCycleEncoder encoder;
+  /**
+   * Inversion state.
+   */
+  private       boolean          isInverted;
+  /**
+   * An {@link Alert}  for if the encoder cannot report accurate velocities.
+   */
+  private       Alert            inaccurateVelocities;
+  /**
+   * The Offset in degrees of the DIO absolute encoder.
+   */
+  private       double           offset;
+
+  /**
+   * Constructor for the DIO duty cycle encoder.
+   *
+   * @param pin DIO lane for the encoder.
+   */
+  public DIODutyCycleEncoderSwerve(int pin)
+  {
+    encoder = new DutyCycleEncoder(pin);
+    Timer.delay(2);
+    inaccurateVelocities = new Alert(
+        "Encoders",
+        "The DIO Duty Cycle encoder may not report accurate velocities!",
+        AlertType.kWarning);
+
+  }
+
+  @Override
+  public void close()
+  {
+    encoder.close();
+  }
+
+  /**
+   * Configure the inversion state of the encoder.
+   *
+   * @param inverted Whether the encoder is inverted.
+   */
+  @Override
+  public void configure(boolean inverted)
+  {
+    isInverted = inverted;
+  }
+
+  /**
+   * Get the absolute position of the encoder.
+   *
+   * @return Absolute position in degrees from [0, 360).
+   */
+  @Override
+  public double getAbsolutePosition()
+  {
+    return (isInverted ? -1.0 : 1.0) * ((encoder.get() * 360) - offset);
+  }
+
+  /**
+   * Get the encoder object.
+   *
+   * @return {@link DutyCycleEncoder} from the class.
+   */
+  @Override
+  public Object getAbsoluteEncoder()
+  {
+    return encoder;
+  }
+
+  /**
+   * Get the velocity in degrees/sec.
+   *
+   * @return velocity in degrees/sec.
+   */
+  @Override
+  public double getVelocity()
+  {
+    inaccurateVelocities.set(true);
+    return encoder.get();
+  }
+
+  /**
+   * Reset the encoder to factory defaults.
+   */
+  @Override
+  public void factoryDefault()
+  {
+    // Do nothing
+  }
+
+  /**
+   * Clear sticky faults on the encoder.
+   */
+  @Override
+  public void clearStickyFaults()
+  {
+    // Do nothing
+  }
+
+
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    this.offset = offset;
+
+    return true;
+  }
+}

swervelib/encoders/SparkFlexEncoderSwerve.java

@@ -0,0 +1,165 @@
+package swervelib.encoders;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.spark.SparkAbsoluteEncoder;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import swervelib.motors.SparkFlexSwerve;
+import swervelib.motors.SwerveMotor;
+
+/**
+ * SparkFlex absolute encoder, attached through the data port.
+ */
+public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder
+{
+
+  /**
+   * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkFlex.
+   */
+  public  SparkAbsoluteEncoder encoder;
+  /**
+   * An {@link Alert} for if there is a failure configuring the encoder.
+   */
+  private Alert                failureConfiguring;
+  /**
+   * {@link SparkFlexSwerve} instance.
+   */
+  private SwerveMotor sparkFlex;
+
+  /**
+   * Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex} motor.
+   *
+   * @param motor            Motor to create the encoder from.
+   * @param conversionFactor The conversion factor to set if the output is not from 0 to 360.
+   */
+  public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor)
+  {
+    failureConfiguring = new Alert(
+        "Encoders",
+        "Failure configuring SparkFlex Absolute Encoder",
+        AlertType.kWarning);
+    if (motor.getMotor() instanceof SparkFlex)
+    {
+      sparkFlex = motor;
+      encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder();
+      setConversionFactor(conversionFactor);
+    } else
+    {
+      throw new RuntimeException("Motor given to instantiate SparkFlexEncoder is not a CANSparkFlex");
+    }
+  }
+
+  @Override
+  public void close()
+  {
+    // SPARK Flex encoder gets closed with the motor
+    // I don't think an encoder getting closed should 
+    // close the entire motor so i will keep this empty
+    // sparkFlex.close();
+  }
+
+  /**
+   * Reset the encoder to factory defaults.
+   */
+  @Override
+  public void factoryDefault()
+  {
+    // Do nothing
+  }
+
+  /**
+   * Clear sticky faults on the encoder.
+   */
+  @Override
+  public void clearStickyFaults()
+  {
+    // Do nothing
+  }
+
+  /**
+   * Configure the absolute encoder to read from [0, 360) per second.
+   *
+   * @param inverted Whether the encoder is inverted.
+   */
+  @Override
+  public void configure(boolean inverted)
+  {
+    if (sparkFlex instanceof SparkFlexSwerve)
+    {
+      SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
+      cfg.absoluteEncoder.inverted(inverted);
+      ((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
+    }
+  }
+
+  /**
+   * Set the conversion factor of the {@link SparkFlexEncoderSwerve}.
+   *
+   * @param conversionFactor Position conversion factor from ticks to unit.
+   */
+  public void setConversionFactor(double conversionFactor)
+  {
+    SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
+    cfg.signals
+        .absoluteEncoderPositionAlwaysOn(true)
+        .absoluteEncoderPositionPeriodMs(20);
+    cfg.absoluteEncoder
+        .positionConversionFactor(conversionFactor)
+        .velocityConversionFactor(conversionFactor / 60);
+    ((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
+  }
+
+  /**
+   * Get the absolute position of the encoder.
+   *
+   * @return Absolute position in degrees from [0, 360).
+   */
+  @Override
+  public double getAbsolutePosition()
+  {
+    return encoder.getPosition();
+  }
+
+  /**
+   * Get the instantiated absolute encoder Object.
+   *
+   * @return Absolute encoder object.
+   */
+  @Override
+  public Object getAbsoluteEncoder()
+  {
+    return encoder;
+  }
+
+  /**
+   * Sets the Absolute Encoder Offset inside of the SparkFlex's Memory.
+   *
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * @return if setting Absolute Encoder Offset was successful or not.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    if (sparkFlex instanceof SparkFlexSwerve)
+    {
+      SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
+      cfg.absoluteEncoder.zeroOffset(offset);
+      ((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
+      return true;
+    }
+    return false;
+  }
+
+  /**
+   * Get the velocity in degrees/sec.
+   *
+   * @return velocity in degrees/sec.
+   */
+  @Override
+  public double getVelocity()
+  {
+    return encoder.getVelocity();
+  }
+}

swervelib/encoders/SparkMaxAnalogEncoderSwerve.java

@@ -3,6 +3,7 @@ package swervelib.encoders;
 import com.revrobotics.REVLibError;
 import com.revrobotics.spark.SparkAnalogSensor;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -47,8 +48,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
     {
       sparkMax = motor;
       encoder = ((SparkMax) motor.getMotor()).getAnalog();
-      motor.setAbsoluteEncoder(this);
+      setConversionFactor(360.0 / maxVoltage);
-      sparkMax.configureIntegratedEncoder(360 / maxVoltage);
     } else
     {
       throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
@@ -64,6 +64,15 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
 
   }
 
+  @Override
+  public void close()
+  {
+    // SPARK MAX Analog encoder gets closed with the motor
+    // I don't think an encoder getting closed should 
+    // close the entire motor so i will keep this empty
+    // sparkMax.close();
+  }
+
   /**
    * Run the configuration until it succeeds or times out.
    *
@@ -81,6 +90,49 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
     failureConfiguring.set(true);
   }
 
+  /**
+   * Set the conversion factor of the {@link SparkMaxAnalogEncoderSwerve}.
+   *
+   * @param conversionFactor Position conversion factor from ticks to unit.
+   */
+  public void setConversionFactor(double conversionFactor)
+  {
+    SparkMaxConfig cfg = null;
+    if (sparkMax instanceof SparkMaxSwerve)
+    {
+      cfg = ((SparkMaxSwerve) sparkMax).getConfig();
+
+    } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+    {
+      cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
+    }
+    if (cfg != null)
+    {
+      cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
+
+      cfg.signals
+          .analogVelocityAlwaysOn(true)
+          .analogVoltageAlwaysOn(true)
+          .analogPositionAlwaysOn(true)
+          .analogVoltagePeriodMs(20)
+          .analogPositionPeriodMs(20)
+          .analogVelocityPeriodMs(20);
+
+      cfg.analogSensor
+          .positionConversionFactor(conversionFactor)
+          .velocityConversionFactor(conversionFactor / 60);
+    }
+    if (sparkMax instanceof SparkMaxSwerve)
+    {
+      ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
+    } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+    {
+      ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
+    }
+
+  }
+
+
   /**
    * Reset the encoder to factory defaults.
    */
@@ -110,12 +162,12 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
     if (sparkMax instanceof SparkMaxSwerve)
     {
       SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
-      cfg.analogSensor.inverted(true);
+      cfg.analogSensor.inverted(inverted);
       ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
     } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
     {
       SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
-      cfg.analogSensor.inverted(true);
+      cfg.analogSensor.inverted(inverted);
       ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
     }
   }

swervelib/encoders/SparkMaxEncoderSwerve.java

@@ -4,6 +4,7 @@ import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.REVLibError;
 import com.revrobotics.spark.SparkAbsoluteEncoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -46,7 +47,7 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
   {
     failureConfiguring = new Alert(
         "Encoders",
-        "Failure configuring SparkMax Analog Encoder",
+        "Failure configuring SparkMax Absolute Encoder",
         AlertType.kWarning);
     offsetFailure = new Alert(
         "Encoders",
@@ -56,14 +57,22 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
     {
       sparkMax = motor;
       encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder();
-      motor.setAbsoluteEncoder(this);
+      setConversionFactor(conversionFactor);
-      motor.configureIntegratedEncoder(conversionFactor);
     } else
     {
       throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
     }
   }
 
+  @Override
+  public void close()
+  {
+    // SPARK MAX encoder gets closed with the motor
+    // I don't think an encoder getting closed should 
+    // close the entire motor so i will keep this empty
+    // sparkFlex.close();
+  }
+
   /**
    * Run the configuration until it succeeds or times out.
    *
@@ -110,16 +119,62 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
     if (sparkMax instanceof SparkMaxSwerve)
     {
       SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
-      cfg.analogSensor.inverted(true);
+      cfg.absoluteEncoder.inverted(inverted);
       ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
     } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
     {
       SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
-      cfg.analogSensor.inverted(true);
+      cfg.absoluteEncoder.inverted(inverted);
       ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
     }
   }
 
+
+  /**
+   * Set the conversion factor of the {@link SparkMaxEncoderSwerve}.
+   *
+   * @param conversionFactor Position conversion factor from ticks to unit.
+   */
+  public void setConversionFactor(double conversionFactor)
+  {
+    // 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.
+
+    SparkMaxConfig cfg = null;
+    if (sparkMax instanceof SparkMaxSwerve)
+    {
+      cfg = ((SparkMaxSwerve) sparkMax).getConfig();
+
+    } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+    {
+      cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
+    }
+    if (cfg != null)
+    {
+      cfg.signals
+          .absoluteEncoderPositionAlwaysOn(true)
+          .absoluteEncoderPositionPeriodMs(20);
+
+      cfg.absoluteEncoder
+          .positionConversionFactor(conversionFactor)
+          .velocityConversionFactor(conversionFactor / 60);
+    }
+    if (sparkMax instanceof SparkMaxSwerve)
+    {
+      ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
+    } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+    {
+      ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
+    }
+
+  }
+
+
   /**
    * Get the absolute position of the encoder.
    *
@@ -151,19 +206,6 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
   @Override
   public boolean setAbsoluteEncoderOffset(double offset)
   {
-    if (sparkMax instanceof SparkMaxSwerve)
-    {
-      SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
-      cfg.absoluteEncoder.zeroOffset(offset);
-      ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
-      return true;
-    } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
-    {
-      SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
-      cfg.absoluteEncoder.zeroOffset(offset);
-      ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
-      return true;
-    }
     return false;
   }
 

swervelib/encoders/SwerveAbsoluteEncoder.java

@@ -3,9 +3,15 @@ package swervelib.encoders;
 /**
  * Swerve abstraction class to define a standard interface with absolute encoders for swerve modules..
  */
-public abstract class SwerveAbsoluteEncoder
+public abstract class SwerveAbsoluteEncoder implements AutoCloseable
 {
 
+  // This is a bit weird because some encoders are closable 
+  // while some get closed with the motor controller
+  // so for some encoders this will be an empty function
+  @Override
+  public abstract void close();
+
   /**
    * The maximum amount of times the swerve encoder will attempt to configure itself if failures occur.
    */

swervelib/encoders/TalonSRXEncoderSwerve.java

@@ -44,6 +44,15 @@ public class TalonSRXEncoderSwerve extends SwerveAbsoluteEncoder
     }
   }
 
+  @Override
+  public void close()
+  {
+    // TalonSRX encoder gets closed with the motor
+    // I don't think an encoder getting closed should 
+    // close the entire motor so i will keep this empty
+    // sparkFlex.close();
+  }
+
   @Override
   public void factoryDefault()
   {

swervelib/encoders/ThriftyNovaEncoderSwerve.java

@@ -6,89 +6,111 @@ import swervelib.motors.ThriftyNovaSwerve;
 /**
  * Thrifty Nova absolute encoder, attached through the data port.
  */
-public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder {
+public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder
-    /** The absolute encoder is directly interfaced through the Thrifty Nova motor. */
+{
-    protected ThriftyNovaSwerve motor;
-    /**
-     * Inversion state of the attached encoder.
-     */
-    protected boolean inverted = false;
-    /**
-     * Offset of the absolute encoder.
-     */
-    protected double offset = 0.0;
-   
-    /**
-     * Create the {@link ThriftyNovaEncoderSwerve} object as an absolute encoder from the {@link ThriftyNovaSwerve} motor.
-     *
-     * @param motor {@link SwerveMotor} through which to interface with the attached encoder .
-     */
-    public ThriftyNovaEncoderSwerve(SwerveMotor motor) {
-        this.motor = (ThriftyNovaSwerve)motor;
-        motor.setAbsoluteEncoder(null);
-    }
 
-    /**
+  /**
-     * Set factory default.
+   * The absolute encoder is directly interfaced through the Thrifty Nova motor.
-     */
+   */
-    @Override
+  protected ThriftyNovaSwerve motor;
-    public void factoryDefault() {
+  /**
-    }
+   * Inversion state of the attached encoder.
+   */
+  protected boolean           inverted = false;
+  /**
+   * Offset of the absolute encoder.
+   */
+  protected double            offset   = 0.0;
 
-    /**
+  /**
-     * Clear sticky faults.
+   * Create the {@link ThriftyNovaEncoderSwerve} object as an absolute encoder from the {@link ThriftyNovaSwerve}
-     */
+   * motor.
-    @Override
+   *
-    public void clearStickyFaults() {
+   * @param motor {@link SwerveMotor} through which to interface with the attached encoder .
-    }
+   */
+  public ThriftyNovaEncoderSwerve(SwerveMotor motor)
+  {
+    this.motor = (ThriftyNovaSwerve) motor;
+    motor.setAbsoluteEncoder(null);
+  }
 
-    /**
+  @Override
-     * Configure the absolute encoder.
+  public void close()
-     *
+  {
-     * @param inverted Whether the encoder is inverted.
+    // ThriftyNova encoder gets closed with the motor
-     */
+    // I don't think an encoder getting closed should 
-    @Override
+    // close the entire motor so i will keep this empty
-    public void configure(boolean inverted) {
+    // sparkFlex.close();
-        this.inverted = inverted;
+  }
-    }
 
-    /**
+  /**
-     * Get the absolute position of the encoder.
+   * Set factory default.
-     *
+   */
-     * @return Absolute position in degrees from [0, 360).  
+  @Override
-     */
+  public void factoryDefault()
-    @Override
+  {
-    public double getAbsolutePosition() {
+  }
-        return (motor.getPosition() + offset) * (inverted ? -1.0 : 1.0);
-    }
 
-    /**
+  /**
-     * Get the instantiated absolute encoder Object.
+   * Clear sticky faults.
-     */
+   */
-    @Override
+  @Override
-    public Object getAbsoluteEncoder() {
+  public void clearStickyFaults()
-        return null;
+  {
-    }
+  }
 
-    /**
+  /**
-     * Set the absolute encoder offset.
+   * Configure the absolute encoder.
-     *
+   *
-     * @param offset offset in degrees from [0, 360).
+   * @param inverted Whether the encoder is inverted.
-     * @return true if successful.
+   */
-     */
+  @Override
-    @Override
+  public void configure(boolean inverted)
-    public boolean setAbsoluteEncoderOffset(double offset) {
+  {
-        this.offset = offset;
+    this.inverted = inverted;
-        return true;
+  }
-    }
 
-    /**
+  /**
-     * Get the absolute encoder velocity.
+   * Get the absolute position of the encoder.
-     * WARNING: Angular velocity is generally not measurable at high speeds.
+   *
-     * @return Velocity in degrees per second.
+   * @return Absolute position in degrees from [0, 360).
-     */
+   */
-    @Override
+  @Override
-    public double getVelocity() {
+  public double getAbsolutePosition()
-       return motor.getVelocity();
+  {
-    }    
+    return (motor.getPosition() + offset) * (inverted ? -1.0 : 1.0);
+  }
+
+  /**
+   * Get the instantiated absolute encoder Object.
+   */
+  @Override
+  public Object getAbsoluteEncoder()
+  {
+    return null;
+  }
+
+  /**
+   * Set the absolute encoder offset.
+   *
+   * @param offset offset in degrees from [0, 360).
+   * @return true if successful.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    this.offset = offset;
+    return true;
+  }
+
+  /**
+   * Get the absolute encoder velocity. WARNING: Angular velocity is generally not measurable at high speeds.
+   *
+   * @return Velocity in degrees per second.
+   */
+  @Override
+  public double getVelocity()
+  {
+    return motor.getVelocity();
+  }
 }

swervelib/imu/ADIS16448Swerve.java

@@ -20,7 +20,7 @@ public class ADIS16448Swerve extends SwerveIMU
    */
   private final ADIS16448_IMU      imu;
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
   private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
   /**
@@ -42,6 +42,11 @@ public class ADIS16448Swerve extends SwerveIMU
     SmartDashboard.putData(imu);
   }
 
+  @Override
+  public void close() {
+    imu.close();
+  }
+
   /**
    * Reset IMU to factory default.
    */

swervelib/imu/ADIS16470Swerve.java

@@ -21,7 +21,7 @@ public class ADIS16470Swerve extends SwerveIMU
    */
   private final ADIS16470_IMU      imu;
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
   private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
   /**
@@ -44,6 +44,11 @@ public class ADIS16470Swerve extends SwerveIMU
     SmartDashboard.putData(imu);
   }
 
+  @Override
+  public void close() {
+    imu.close();
+  }
+
   /**
    * Reset IMU to factory default.
    */

swervelib/imu/ADXRS450Swerve.java

@@ -20,7 +20,7 @@ public class ADXRS450Swerve extends SwerveIMU
    */
   private final ADXRS450_Gyro      imu;
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
   private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
   /**
@@ -42,6 +42,11 @@ public class ADXRS450Swerve extends SwerveIMU
     SmartDashboard.putData(imu);
   }
 
+  @Override
+  public void close() {
+    imu.close();
+  }
+
   /**
    * Reset IMU to factory default.
    */

swervelib/imu/AnalogGyroSwerve.java

@@ -20,7 +20,7 @@ public class AnalogGyroSwerve extends SwerveIMU
    */
   private final AnalogGyro         imu;
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
   private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
   /**
@@ -49,6 +49,11 @@ public class AnalogGyroSwerve extends SwerveIMU
     SmartDashboard.putData(imu);
   }
 
+  @Override
+  public void close() {
+    imu.close();
+  }
+
   /**
    * Reset IMU to factory default.
    */

swervelib/imu/CanandgyroSwerve.java

@@ -6,7 +6,6 @@ import com.reduxrobotics.sensors.canandgyro.Canandgyro;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.measure.MutAngularVelocity;
-
 import java.util.Optional;
 
 /**
@@ -24,7 +23,7 @@ public class CanandgyroSwerve extends SwerveIMU
    */
   private final Canandgyro         imu;
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
   private final MutAngularVelocity yawVel                 = new MutAngularVelocity(0, 0, RotationsPerSecond);
   /**
@@ -46,6 +45,12 @@ public class CanandgyroSwerve extends SwerveIMU
     imu = new Canandgyro(canid);
   }
 
+  @Override
+  public void close()
+  {
+    imu.close();
+  }
+
   /**
    * Reset {@link Canandgyro} to factory default.
    */

swervelib/imu/NavXSwerve.java

@@ -18,7 +18,7 @@ public class NavXSwerve extends SwerveIMU
 {
 
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
   private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
   /**
@@ -60,6 +60,10 @@ public class NavXSwerve extends SwerveIMU
     }
   }
 
+  @Override
+  public void close() {
+    imu.close();
+  }
 
   /**
    * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be

swervelib/imu/Pigeon2Swerve.java

@@ -23,41 +23,41 @@ public class Pigeon2Swerve extends SwerveIMU
   /**
    * Wait time for status frames to show up.
    */
-  public static double             STATUS_TIMEOUT_SECONDS = 0.04;
+  public static double              STATUS_TIMEOUT_SECONDS = 0.04;
   /**
    * {@link Pigeon2} IMU device.
    */
-  private final Pigeon2            imu;
+  private final Pigeon2             imu;
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
-  private final MutAngularVelocity yawVel                 = new MutAngularVelocity(0, 0, DegreesPerSecond);
+  private final MutAngularVelocity  yawVel                 = new MutAngularVelocity(0,
-
+                                                                                    0,
-  /**
+                                                                                    DegreesPerSecond);
-   * Offset for the {@link Pigeon2}.
-   */
-  private Rotation3d          offset      = new Rotation3d();
-  /**
-   * Inversion for the gyro
-   */
-  private boolean             invertedIMU = false;
-  /**
-   * {@link Pigeon2} configurator.
-   */
-  private Pigeon2Configurator cfg;
-
   /**
    * X Acceleration supplier
    */
-  private Supplier<StatusSignal<LinearAcceleration>> xAcc;
+  private final Supplier<StatusSignal<LinearAcceleration>> xAcc;
   /**
    * Y Accelleration supplier.
    */
-  private Supplier<StatusSignal<LinearAcceleration>> yAcc;
+  private final Supplier<StatusSignal<LinearAcceleration>> yAcc;
   /**
    * Z Acceleration supplier.
    */
-  private Supplier<StatusSignal<LinearAcceleration>> zAcc;
+  private final Supplier<StatusSignal<LinearAcceleration>> zAcc;
+  /**
+   * Offset for the {@link Pigeon2}.
+   */
+  private       Rotation3d          offset                 = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private       boolean             invertedIMU            = false;
+  /**
+   * {@link Pigeon2} configurator.
+   */
+  private       Pigeon2Configurator cfg;
 
   /**
    * Generate the SwerveIMU for {@link Pigeon2}.
@@ -85,6 +85,12 @@ public class Pigeon2Swerve extends SwerveIMU
     this(canid, "");
   }
 
+  @Override
+  public void close() {
+    imu.close();
+  }
+
+
   /**
    * Reset {@link Pigeon2} to factory default.
    */
@@ -159,15 +165,15 @@ public class Pigeon2Swerve extends SwerveIMU
   @Override
   public Optional<Translation3d> getAccel()
   {
-    // TODO: Implement later.
+    return Optional.of(new Translation3d(xAcc.get().getValueAsDouble(),
-
+                                         yAcc.get().getValueAsDouble(),
-    return Optional.empty();
+                                         zAcc.get().getValueAsDouble()));
   }
 
   @Override
   public MutAngularVelocity getYawAngularVelocity()
   {
-    return yawVel.mut_replace(imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue());
+    return yawVel.mut_replace(imu.getAngularVelocityZWorld().refresh().getValue());
   }
 
   /**

swervelib/imu/PigeonSwerve.java

@@ -21,7 +21,7 @@ public class PigeonSwerve extends SwerveIMU
    */
   private final WPI_PigeonIMU      imu;
   /**
-   * Mutable {@link AngularVelocity} for readings.
+   * Mutable {@link MutAngularVelocity} for readings.
    */
   private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
   /**
@@ -45,6 +45,12 @@ public class PigeonSwerve extends SwerveIMU
     SmartDashboard.putData(imu);
   }
 
+  @Override
+  public void close() {
+    imu.close();
+  }
+
+
   /**
    * Reset IMU to factory default.
    */

swervelib/imu/PigeonViaTalonSRXSwerve.java

@@ -0,0 +1,158 @@
+package swervelib.imu;
+
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.Optional;
+
+/**
+ * SwerveIMU interface for the {@link WPI_PigeonIMU}.
+ */
+public class PigeonViaTalonSRXSwerve extends SwerveIMU
+{
+
+
+  /**
+   * {@link TalonSRX} TalonSRX the IMU is attached to.
+   */
+  private final WPI_TalonSRX talon;
+
+  /**
+   * {@link WPI_PigeonIMU} IMU device.
+   */
+  private final WPI_PigeonIMU      imu;
+  /**
+   * Mutable {@link AngularVelocity} for readings.
+   */
+  private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
+  /**
+   * Offset for the {@link WPI_PigeonIMU}.
+   */
+  private       Rotation3d         offset      = new Rotation3d();
+  /**
+   * Inversion for the gyro
+   */
+  private       boolean            invertedIMU = false;
+
+  /**
+   * Generate the SwerveIMU for {@link WPI_PigeonIMU} attached to a {@link TalonSRX}.
+   *
+   * @param canid CAN ID for the {@link TalonSRX} the {@link WPI_PigeonIMU} is attached to, does not support CANBus.
+   */
+  public PigeonViaTalonSRXSwerve(int canid)
+  {
+    talon = new WPI_TalonSRX(canid);
+    imu = new WPI_PigeonIMU(talon);
+    offset = new Rotation3d();
+    SmartDashboard.putData(imu);
+  }
+
+  @Override
+  public void close() {
+    imu.close();
+    talon.close();
+  }
+
+  /**
+   * Reset IMU to factory default.
+   */
+  @Override
+  public void factoryDefault()
+  {
+    imu.configFactoryDefault();
+  }
+
+  /**
+   * Clear sticky faults on IMU.
+   */
+  @Override
+  public void clearStickyFaults()
+  {
+    imu.clearStickyFaults();
+  }
+
+  /**
+   * Set the gyro offset.
+   *
+   * @param offset gyro offset as a {@link Rotation3d}.
+   */
+  public void setOffset(Rotation3d offset)
+  {
+    this.offset = offset;
+  }
+
+  /**
+   * Set the gyro to invert its default direction
+   *
+   * @param invertIMU invert gyro direction
+   */
+  public void setInverted(boolean invertIMU)
+  {
+    invertedIMU = invertIMU;
+  }
+
+  /**
+   * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
+   *
+   * @return {@link Rotation3d} from the IMU.
+   */
+  @Override
+  public Rotation3d getRawRotation3d()
+  {
+    double[] wxyz = new double[4];
+    imu.get6dQuaternion(wxyz);
+    Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
+    return invertedIMU ? reading.unaryMinus() : reading;
+  }
+
+  /**
+   * Fetch the {@link Rotation3d} from the IMU. Robot relative.
+   *
+   * @return {@link Rotation3d} from the IMU.
+   */
+  @Override
+  public Rotation3d getRotation3d()
+  {
+    return getRawRotation3d().minus(offset);
+  }
+
+  /**
+   * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
+   * empty.
+   *
+   * @return {@link Translation3d} of the acceleration as an {@link Optional}.
+   */
+  @Override
+  public Optional<Translation3d> getAccel()
+  {
+    short[] initial = new short[3];
+    imu.getBiasedAccelerometer(initial);
+    return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
+  }
+
+  @Override
+  public MutAngularVelocity getYawAngularVelocity()
+  {
+    return yawVel.mut_setMagnitude(imu.getRate());
+  }
+
+  /**
+   * Get the instantiated {@link WPI_PigeonIMU} IMU object.
+   *
+   * @return IMU object.
+   */
+  @Override
+  public Object getIMU()
+  {
+    return imu;
+  }
+
+}

swervelib/imu/SwerveIMU.java

@@ -3,15 +3,17 @@ package swervelib.imu;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.measure.MutAngularVelocity;
-
 import java.util.Optional;
 
 /**
  * Swerve IMU abstraction to define a standard interface with a swerve drive.
  */
-public abstract class SwerveIMU
+public abstract class SwerveIMU implements AutoCloseable
 {
 
+  @Override
+  public abstract void close();
+
   /**
    * Reset IMU to factory default.
    */

swervelib/math/SwerveMath.java

@@ -95,7 +95,7 @@ public class SwerveMath
         optimalVoltage
         / calculateMaxAcceleration(wheelGripCoefficientOfFriction);
     /// ^ Volt-seconds^2 per meter (max voltage divided by max accel)
-    return new SimpleMotorFeedforward(0, kv, ka);
+    return new SimpleMotorFeedforward(0, kv, 0);
   }
 
   /**

swervelib/motors/SparkFlexSwerve.java

@@ -6,22 +6,22 @@ import static edu.wpi.first.units.Units.Seconds;
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkFlexConfig;
-import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.Optional;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.parser.PIDFConfig;
@@ -33,46 +33,42 @@ import swervelib.telemetry.SwerveDriveTelemetry;
 public class SparkFlexSwerve extends SwerveMotor
 {
 
+  /**
+   * Config retry delay.
+   */
+  private final double                          configDelay     = Milliseconds.of(5).in(Seconds);
   /**
    * {@link SparkFlex} Instance.
    */
-  private final SparkFlex                 motor;
+  private final SparkFlex                       motor;
   /**
    * Integrated encoder.
    */
-  public        RelativeEncoder           encoder;
+  public        RelativeEncoder                 encoder;
   /**
    * Absolute encoder attached to the SparkFlex (if exists)
    */
-  public        SwerveAbsoluteEncoder     absoluteEncoder;
+  public        Optional<SwerveAbsoluteEncoder> absoluteEncoder = Optional.empty();
   /**
    * Closed-loop PID controller.
    */
-  public        SparkClosedLoopController pid;
+  public        SparkClosedLoopController       pid;
   /**
    * Supplier for the velocity of the motor controller.
    */
-  private       Supplier<Double>          velocity;
+  private       Supplier<Double>                velocity;
   /**
    * Supplier for the position of the motor controller.
    */
-  private       Supplier<Double>          position;
+  private       Supplier<Double>                position;
-  /**
-   * Factory default already occurred.
-   */
-  private       boolean                   factoryDefaultOccurred = false;
   /**
    * An {@link Alert} for if there is an error configuring the motor.
    */
-  private       Alert                     failureConfiguring;
+  private       Alert                           failureConfiguring;
-  /**
-   * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client.
-   */
-  private       Alert                     absoluteEncoderOffsetWarning;
   /**
    * Configuration object for {@link SparkFlex} motor.
    */
-  private       SparkFlexConfig           cfg                    = new SparkFlexConfig();
+  private       SparkFlexConfig                 cfg             = new SparkFlexConfig();
 
 
   /**
@@ -86,6 +82,10 @@ public class SparkFlexSwerve extends SwerveMotor
   {
     this.motor = motor;
     this.isDriveMotor = isDriveMotor;
+    failureConfiguring = new Alert("Motors",
+                                   "Failure configuring motor " +
+                                   motor.getDeviceId(),
+                                   AlertType.kWarning);
     factoryDefaults();
     clearStickyFaults();
 
@@ -95,14 +95,7 @@ public class SparkFlexSwerve extends SwerveMotor
 
     // Spin off configurations in a different thread.
     // configureSparkFlex(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
-    failureConfiguring = new Alert("Motors",
+
-                                   "Failure configuring motor " +
-                                   motor.getDeviceId(),
-                                   AlertType.kWarning);
-    absoluteEncoderOffsetWarning = new Alert("Motors",
-                                             "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " +
-                                             "absoluteEncoderOffset in the Swerve Module JSON!",
-                                             AlertType.kWarning);
     velocity = encoder::getVelocity;
     position = encoder::getPosition;
   }
@@ -132,11 +125,17 @@ public class SparkFlexSwerve extends SwerveMotor
       {
         return;
       }
-      Timer.delay(Milliseconds.of(5).in(Seconds));
+      Timer.delay(configDelay);
     }
     failureConfiguring.set(true);
   }
 
+  @Override
+  public void close()
+  {
+    motor.close();
+  }
+
   /**
    * Get the current configuration of the {@link SparkFlex}
    *
@@ -230,9 +229,9 @@ public class SparkFlexSwerve extends SwerveMotor
    * @return connected absolute encoder state.
    */
   @Override
-  public boolean isAttachedAbsoluteEncoder()
+  public boolean usingExternalFeedbackSensor()
   {
-    return absoluteEncoder != null;
+    return absoluteEncoder.isPresent();
   }
 
   /**
@@ -264,7 +263,7 @@ public class SparkFlexSwerve extends SwerveMotor
   {
     if (encoder == null)
     {
-      absoluteEncoder = null;
+      this.absoluteEncoder = Optional.empty();
       cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
       velocity = this.encoder::getVelocity;
@@ -272,11 +271,10 @@ public class SparkFlexSwerve extends SwerveMotor
     } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
     {
       cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
-      absoluteEncoderOffsetWarning.set(true);
+      this.absoluteEncoder = Optional.of(encoder);
-      absoluteEncoder = encoder;
 
-      velocity = absoluteEncoder::getVelocity;
+      velocity = this.absoluteEncoder.get()::getVelocity;
-      position = absoluteEncoder::getAbsolutePosition;
+      position = this.absoluteEncoder.get()::getAbsolutePosition;
     }
     return this;
   }
@@ -301,56 +299,31 @@ public class SparkFlexSwerve extends SwerveMotor
         .primaryEncoderVelocityAlwaysOn(false)
         .iAccumulationAlwaysOn(false)
         .appliedOutputPeriodMs(10)
-        .faultsPeriodMs(20)
+        .faultsPeriodMs(20);
-    ;
+    cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
-    if (absoluteEncoder == null)
-    {
-      cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
-      cfg.encoder
+    cfg.encoder
-          .positionConversionFactor(positionConversionFactor)
+        .positionConversionFactor(positionConversionFactor)
-          .velocityConversionFactor(positionConversionFactor / 60);
+        .velocityConversionFactor(positionConversionFactor / 60);
-      // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
+    // 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)
+    // 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
+    // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
-      // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
+    // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
-      // This value was taken from:
+    // 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
+    // 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
+    // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
-      cfg.encoder
+    cfg.encoder
-          .quadratureMeasurementPeriod(10)
+        .quadratureMeasurementPeriod(10)
-          .quadratureAverageDepth(2);
+        .quadratureAverageDepth(2);
 
-      // Taken from
+    // Taken from
-      // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
+    // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
-      // Unused frames can be set to 65535 to decrease CAN ultilization.
+    // Unused frames can be set to 65535 to decrease CAN ultilization.
-      cfg.signals
+    cfg.signals
-          .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
+        .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
-          .primaryEncoderPositionAlwaysOn(true)
+        .primaryEncoderPositionAlwaysOn(true)
-          .primaryEncoderPositionPeriodMs(20);
+        .primaryEncoderPositionPeriodMs(20);
 
-    } 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.
-      if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
-      {
-        cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
-
-        cfg.signals
-            .absoluteEncoderPositionAlwaysOn(true)
-            .absoluteEncoderPositionPeriodMs(20);
-
-        cfg.absoluteEncoder
-            .positionConversionFactor(positionConversionFactor)
-            .velocityConversionFactor(positionConversionFactor / 60);
-      }
-    }
 
   }
 
@@ -382,6 +355,15 @@ public class SparkFlexSwerve extends SwerveMotor
 
   }
 
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  @Override
+  public void disablePIDWrapping()
+  {
+    cfg.closedLoop.positionWrappingEnabled(false);
+  }
+
   /**
    * Set the idle mode.
    *
@@ -539,7 +521,7 @@ public class SparkFlexSwerve extends SwerveMotor
   @Override
   public void setPosition(double position)
   {
-    if (absoluteEncoder == null)
+    if (absoluteEncoder.isEmpty())
     {
       configureSparkFlex(() -> encoder.setPosition(position));
     }

swervelib/motors/SparkMaxBrushedMotorSwerve.java

@@ -3,13 +3,12 @@ package swervelib.motors;
 import static edu.wpi.first.units.Units.Milliseconds;
 import static edu.wpi.first.units.Units.Seconds;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
@@ -35,50 +34,50 @@ import swervelib.telemetry.SwerveDriveTelemetry;
 public class SparkMaxBrushedMotorSwerve extends SwerveMotor
 {
 
+  /**
+   * Config retry delay.
+   */
+  private final double                    configDelay = Milliseconds.of(5).in(Seconds);
   /**
    * SparkMAX Instance.
    */
-  private final SparkMax                  motor;
+  private final SparkMax                        motor;
   /**
    * Absolute encoder attached to the SparkMax (if exists)
    */
-  public        SwerveAbsoluteEncoder     absoluteEncoder;
+  public        Optional<SwerveAbsoluteEncoder> absoluteEncoder;
   /**
    * Integrated encoder.
    */
-  public        Optional<RelativeEncoder> encoder                = Optional.empty();
+  public        Optional<RelativeEncoder> encoder     = Optional.empty();
   /**
    * Closed-loop PID controller.
    */
-  public        SparkClosedLoopController pid;
+  public        SparkClosedLoopController       pid;
   /**
    * Supplier for the velocity of the motor controller.
    */
-  private       Supplier<Double>          velocity;
+  private       Supplier<Double>                velocity;
   /**
    * Supplier for the position of the motor controller.
    */
-  private       Supplier<Double>          position;
+  private       Supplier<Double>                position;
-  /**
-   * Factory default already occurred.
-   */
-  private       boolean                   factoryDefaultOccurred = false;
   /**
    * An {@link Alert} for if the motor has no encoder.
    */
-  private       Alert                     noEncoderAlert;
+  private       Alert                           noEncoderAlert;
   /**
    * An {@link Alert} for if there is an error configuring the motor.
    */
-  private       Alert                     failureConfiguringAlert;
+  private       Alert                           failureConfiguringAlert;
   /**
    * An {@link Alert} for if the motor has no encoder defined.
    */
-  private       Alert                     noEncoderDefinedAlert;
+  private       Alert                           noEncoderDefinedAlert;
   /**
    * Configuration object for {@link SparkMax} motor.
    */
-  private       SparkMaxConfig            cfg                    = new SparkMaxConfig();
+  private       SparkMaxConfig            cfg         = new SparkMaxConfig();
 
   /**
    * Initialize the swerve motor.
@@ -187,11 +186,17 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
       {
         return;
       }
-      Timer.delay(Milliseconds.of(5).in(Seconds));
+      Timer.delay(configDelay);
     }
     failureConfiguringAlert.set(true);
   }
 
+  @Override
+  public void close()
+  {
+    motor.close();
+  }
+
   /**
    * Get the current configuration of the {@link SparkMax}
    *
@@ -284,9 +289,9 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
    * @return connected absolute encoder state.
    */
   @Override
-  public boolean isAttachedAbsoluteEncoder()
+  public boolean usingExternalFeedbackSensor()
   {
-    return absoluteEncoder != null;
+    return absoluteEncoder.isPresent();
   }
 
   /**
@@ -318,7 +323,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
   {
     if (encoder == null)
     {
-      absoluteEncoder = null;
+      this.absoluteEncoder = Optional.empty();
       cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
       this.encoder.ifPresentOrElse((RelativeEncoder enc) -> {
@@ -333,16 +338,12 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
       cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
                                     ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
 
-      DriverStation.reportWarning(
+      this.absoluteEncoder = Optional.of(encoder);
-          "IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
+      velocity = this.absoluteEncoder.get()::getVelocity;
-          " absoluteEncoderOffset in the Swerve Module JSON!",
+      position = this.absoluteEncoder.get()::getAbsolutePosition;
-          false);
-      absoluteEncoder = encoder;
-      velocity = absoluteEncoder::getVelocity;
-      position = absoluteEncoder::getAbsolutePosition;
       noEncoderDefinedAlert.set(false);
     }
-    if (absoluteEncoder == null && this.encoder.isEmpty())
+    if (absoluteEncoder.isEmpty() && this.encoder.isEmpty())
     {
       noEncoderDefinedAlert.set(true);
       throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX");
@@ -371,65 +372,29 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
         .iAccumulationAlwaysOn(false)
         .appliedOutputPeriodMs(10)
         .faultsPeriodMs(20);
-    if (absoluteEncoder == null)
-    {
-      cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
-      cfg.encoder
-          .positionConversionFactor(positionConversionFactor)
-          .velocityConversionFactor(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
-      cfg.encoder
-          .quadratureMeasurementPeriod(10)
-          .quadratureAverageDepth(2);
 
-      // Taken from
+    cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
-      // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
+    cfg.encoder
-      // Unused frames can be set to 65535 to decrease CAN ultilization.
+        .positionConversionFactor(positionConversionFactor)
-      cfg.signals
+        .velocityConversionFactor(positionConversionFactor / 60);
-          .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
+    // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
-          .primaryEncoderPositionAlwaysOn(true)
+    // 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)
-          .primaryEncoderPositionPeriodMs(20);
+    // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
-    } else
+    // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
-    {
+    // This value was taken from:
-      // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
+    // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
-      // This needs to be set to 20ms or under to properly update the swerve module position for odometry
+    // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
-      // Configuration taken from 3005, the team who helped develop the Max Swerve:
+    cfg.encoder
-      // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
+        .quadratureMeasurementPeriod(10)
-      // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
+        .quadratureAverageDepth(2);
-      // 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.
-      if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
-      {
-        cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
 
-        cfg.signals
+    // Taken from
-            .absoluteEncoderPositionAlwaysOn(true)
+    // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
-            .absoluteEncoderPositionPeriodMs(20);
+    // Unused frames can be set to 65535 to decrease CAN ultilization.
-
+    cfg.signals
-        cfg.absoluteEncoder
+        .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
-            .positionConversionFactor(positionConversionFactor)
+        .primaryEncoderPositionAlwaysOn(true)
-            .velocityConversionFactor(positionConversionFactor / 60);
+        .primaryEncoderPositionPeriodMs(20);
-      } else
-      {
-        cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
-
-        cfg.signals
-            .analogVoltageAlwaysOn(true)
-            .analogPositionAlwaysOn(true)
-            .analogVoltagePeriodMs(20)
-            .analogPositionPeriodMs(20);
-
-        cfg.analogSensor
-            .positionConversionFactor(positionConversionFactor)
-            .velocityConversionFactor(positionConversionFactor / 60);
-      }
-    }
   }
 
   /**
@@ -460,6 +425,15 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
 
   }
 
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  @Override
+  public void disablePIDWrapping()
+  {
+    cfg.closedLoop.positionWrappingEnabled(false);
+  }
+
   /**
    * Set the idle mode.
    *
@@ -480,6 +454,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
   public void setInverted(boolean inverted)
   {
     cfg.inverted(inverted);
+    if (isDriveMotor)
+    {
+      cfg.encoder.inverted(inverted);
+    }
   }
 
   /**
@@ -620,7 +598,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
   @Override
   public void setPosition(double position)
   {
-    if (absoluteEncoder == null)
+    if (absoluteEncoder.isEmpty())
     {
       encoder.ifPresent((RelativeEncoder enc) -> {
         configureSparkMax(() -> enc.setPosition(position));

swervelib/motors/SparkMaxSwerve.java

@@ -3,7 +3,6 @@ package swervelib.motors;
 import static edu.wpi.first.units.Units.Milliseconds;
 import static edu.wpi.first.units.Units.Seconds;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.ClosedLoopSlot;
@@ -19,6 +18,7 @@ import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.Optional;
 import java.util.function.Supplier;
 import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
 import swervelib.encoders.SparkMaxEncoderSwerve;
@@ -32,38 +32,38 @@ import swervelib.telemetry.SwerveDriveTelemetry;
 public class SparkMaxSwerve extends SwerveMotor
 {
 
+  /**
+   * Config retry delay.
+   */
+  private final double                          configDelay     = Milliseconds.of(5).in(Seconds);
   /**
    * {@link SparkMax} Instance.
    */
-  private final SparkMax                  motor;
+  private final SparkMax                        motor;
   /**
    * Integrated encoder.
    */
-  public        RelativeEncoder           encoder;
+  public        RelativeEncoder                 encoder;
-  /**
-   * Absolute encoder attached to the SparkMax (if exists)
-   */
-  public        SwerveAbsoluteEncoder     absoluteEncoder;
   /**
    * Closed-loop PID controller.
    */
-  public        SparkClosedLoopController pid;
+  public        SparkClosedLoopController       pid;
   /**
-   * Factory default already occurred.
+   * Absolute encoder attached to the SparkMax (if exists)
    */
-  private       boolean                   factoryDefaultOccurred = false;
+  private       Optional<SwerveAbsoluteEncoder> absoluteEncoder = Optional.empty();
   /**
    * Supplier for the velocity of the motor controller.
    */
-  private       Supplier<Double>          velocity;
+  private       Supplier<Double>                velocity;
   /**
    * Supplier for the position of the motor controller.
    */
-  private       Supplier<Double>          position;
+  private       Supplier<Double>                position;
   /**
    * Configuration object for {@link SparkMax} motor.
    */
-  private       SparkMaxConfig            cfg                    = new SparkMaxConfig();
+  private       SparkMaxConfig                  cfg             = new SparkMaxConfig();
 
 
   /**
@@ -118,11 +118,17 @@ public class SparkMaxSwerve extends SwerveMotor
       {
         return;
       }
-      Timer.delay(Milliseconds.of(5).in(Seconds));
+      Timer.delay(configDelay);
     }
     DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
   }
 
+  @Override
+  public void close()
+  {
+    motor.close();
+  }
+
   /**
    * Get the current configuration of the {@link SparkMax}
    *
@@ -217,9 +223,9 @@ public class SparkMaxSwerve extends SwerveMotor
    * @return connected absolute encoder state.
    */
   @Override
-  public boolean isAttachedAbsoluteEncoder()
+  public boolean usingExternalFeedbackSensor()
   {
-    return absoluteEncoder != null;
+    return absoluteEncoder.isPresent();
   }
 
   /**
@@ -251,7 +257,7 @@ public class SparkMaxSwerve extends SwerveMotor
   {
     if (encoder == null)
     {
-      absoluteEncoder = null;
+      this.absoluteEncoder = Optional.empty();
       cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
       velocity = this.encoder::getVelocity;
@@ -262,13 +268,9 @@ public class SparkMaxSwerve extends SwerveMotor
       cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
                                     ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
 
-      DriverStation.reportWarning(
+      this.absoluteEncoder = Optional.of(encoder);
-          "IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
+      velocity = this.absoluteEncoder.get()::getVelocity;
-          " absoluteEncoderOffset in the Swerve Module JSON!",
+      position = this.absoluteEncoder.get()::getAbsolutePosition;
-          false);
-      absoluteEncoder = encoder;
-      velocity = absoluteEncoder::getVelocity;
-      position = absoluteEncoder::getAbsolutePosition;
     }
     return this;
   }
@@ -295,66 +297,29 @@ public class SparkMaxSwerve extends SwerveMotor
         .appliedOutputPeriodMs(10)
         .faultsPeriodMs(20);
 
-    if (absoluteEncoder == null)
+    cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
-    {
+    cfg.encoder
-      cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+        .positionConversionFactor(positionConversionFactor)
-      cfg.encoder
+        .velocityConversionFactor(positionConversionFactor / 60);
-          .positionConversionFactor(positionConversionFactor)
+    // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
-          .velocityConversionFactor(positionConversionFactor / 60);
+    // 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)
-      // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
+    // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
-      // 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)
+    // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
-      // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
+    // This value was taken from:
-      // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
+    // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
-      // This value was taken from:
+    // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
-      // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
+    cfg.encoder
-      // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
+        .quadratureMeasurementPeriod(10)
-      cfg.encoder
+        .quadratureAverageDepth(2);
-          .quadratureMeasurementPeriod(10)
-          .quadratureAverageDepth(2);
 
-      // Taken from
+    // Taken from
-      // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
+    // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
-      // Unused frames can be set to 65535 to decrease CAN ultilization.
+    // Unused frames can be set to 65535 to decrease CAN ultilization.
-      cfg.signals
+    cfg.signals
-          .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
+        .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
-          .primaryEncoderPositionAlwaysOn(true)
+        .primaryEncoderPositionAlwaysOn(true)
-          .primaryEncoderPositionPeriodMs(20);
+        .primaryEncoderPositionPeriodMs(20);
 
-    } 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.
-      if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
-      {
-        cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
-
-        cfg.signals
-            .absoluteEncoderPositionAlwaysOn(true)
-            .absoluteEncoderPositionPeriodMs(20);
-
-        cfg.absoluteEncoder
-            .positionConversionFactor(positionConversionFactor)
-            .velocityConversionFactor(positionConversionFactor / 60);
-      } else
-      {
-        cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
-
-        cfg.signals
-            .analogVoltageAlwaysOn(true)
-            .analogPositionAlwaysOn(true)
-            .analogVoltagePeriodMs(20)
-            .analogPositionPeriodMs(20);
-
-        cfg.analogSensor
-            .positionConversionFactor(positionConversionFactor)
-            .velocityConversionFactor(positionConversionFactor / 60);
-      }
-    }
 
   }
 
@@ -387,6 +352,16 @@ public class SparkMaxSwerve extends SwerveMotor
 
   }
 
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  @Override
+  public void disablePIDWrapping()
+  {
+    cfg.closedLoop
+        .positionWrappingEnabled(false);
+  }
+
   /**
    * Set the idle mode.
    *
@@ -546,7 +521,7 @@ public class SparkMaxSwerve extends SwerveMotor
   @Override
   public void setPosition(double position)
   {
-    if (absoluteEncoder == null)
+    if (absoluteEncoder.isEmpty())
     {
       configureSparkMax(() -> encoder.setPosition(position));
     }

swervelib/motors/SwerveMotor.java

@@ -7,9 +7,12 @@ import swervelib.parser.PIDFConfig;
 /**
  * Swerve motor abstraction which defines a standard interface for motors within a swerve module.
  */
-public abstract class SwerveMotor
+public abstract class SwerveMotor implements AutoCloseable
 {
 
+  @Override
+  public abstract void close();
+
   /**
    * The maximum amount of times the swerve motor will attempt to configure a motor if failures occur.
    */
@@ -65,6 +68,11 @@ public abstract class SwerveMotor
    */
   public abstract void configurePIDWrapping(double minInput, double maxInput);
 
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  public abstract void disablePIDWrapping();
+
   /**
    * Set the idle mode.
    *
@@ -191,5 +199,5 @@ public abstract class SwerveMotor
    *
    * @return connected absolute encoder state.
    */
-  public abstract boolean isAttachedAbsoluteEncoder();
+  public abstract boolean usingExternalFeedbackSensor();
 }

swervelib/motors/TalonFXSSwerve.java

@@ -0,0 +1,467 @@
+package swervelib.motors;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.configs.TalonFXSConfiguration;
+import com.ctre.phoenix6.configs.TalonFXSConfigurator;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFXS;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.system.plant.DCMotor;
+import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.parser.PIDFConfig;
+import swervelib.telemetry.SwerveDriveTelemetry;
+
+/**
+ * {@link TalonFXS} Swerve Motor. Made by Team 1466 WebbRobotics.
+ */
+public class TalonFXSSwerve extends SwerveMotor
+{
+
+  /**
+   * Wait time for status frames to show up.
+   */
+  public static double                STATUS_TIMEOUT_SECONDS  = 0.02;
+  /**
+   * Factory default already occurred.
+   */
+  private final boolean               factoryDefaultOccurred  = false;
+  /**
+   * Whether the absolute encoder is integrated.
+   */
+  private final boolean               absoluteEncoder         = false;
+  /**
+   * Motion magic angle voltage setter.
+   */
+  private final MotionMagicVoltage    m_angleVoltageSetter    = new MotionMagicVoltage(0);
+  /**
+   * Velocity voltage setter for controlling drive motor.
+   */
+  private final VelocityVoltage       m_velocityVoltageSetter = new VelocityVoltage(0);
+  /**
+   * TalonFXS motor controller.
+   */
+  private final TalonFXS              motor;
+  /**
+   * Conversion factor for the motor.
+   */
+  private       double                conversionFactor;
+  /**
+   * Current TalonFXS configuration.
+   */
+  private       TalonFXSConfiguration configuration           = new TalonFXSConfiguration();
+  /**
+   * Current TalonFXS Configurator.
+   */
+  private       TalonFXSConfigurator  cfg;
+
+
+  /**
+   * Constructor for TalonFXS swerve motor.
+   *
+   * @param motor        Motor to use.
+   * @param isDriveMotor Whether this motor is a drive motor.
+   * @param motorType    {@link DCMotor} which the {@link TalonFXS} is attached to.
+   */
+  public TalonFXSSwerve(TalonFXS motor, boolean isDriveMotor, DCMotor motorType)
+  {
+    this.isDriveMotor = isDriveMotor;
+    this.motor = motor;
+    this.cfg = motor.getConfigurator();
+    this.simMotor = motorType;
+
+    factoryDefaults();
+    clearStickyFaults();
+
+  }
+
+  /**
+   * Construct the TalonFXS swerve motor given the ID and CANBus.
+   *
+   * @param id           ID of the TalonFXS on the CANBus.
+   * @param canbus       CANBus on which the TalonFXS is on.
+   * @param isDriveMotor Whether the motor is a drive or steering motor.
+   * @param motorType    {@link DCMotor} which the {@link TalonFXS} is attached to.
+   */
+  public TalonFXSSwerve(int id, String canbus, boolean isDriveMotor, DCMotor motorType)
+  {
+    this(new TalonFXS(id, canbus), isDriveMotor, motorType);
+  }
+
+  /**
+   * Construct the TalonFXS swerve motor given the ID.
+   *
+   * @param id           ID of the TalonFXS on the canbus.
+   * @param isDriveMotor Whether the motor is a drive or steering motor.
+   * @param motorType    {@link DCMotor} which the {@link TalonFXS} is attached to.
+   */
+  public TalonFXSSwerve(int id, boolean isDriveMotor, DCMotor motorType)
+  {
+    this(new TalonFXS(id), isDriveMotor, motorType);
+  }
+
+  /**
+   * Configure the factory defaults.
+   */
+  @Override
+  public void factoryDefaults()
+  {
+    if (!factoryDefaultOccurred)
+    {
+      configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+      configuration.ClosedLoopGeneral.ContinuousWrap = true;
+      cfg.apply(configuration);
+
+      m_angleVoltageSetter.UpdateFreqHz = 0;
+      //      m_angleVoltageExpoSetter.UpdateFreqHz = 0;
+      m_velocityVoltageSetter.UpdateFreqHz = 0;
+      //      motor.configFactoryDefault();
+      //      motor.setSensorPhase(true);
+      //      motor.configSelectedFeedbackSensor(TalonFXSFeedbackDevice.IntegratedSensor, 0, 30);
+      //      motor.configNeutralDeadband(0.001);
+    }
+  }
+
+  /**
+   * Clear the sticky faults on the motor controller.
+   */
+  @Override
+  public void clearStickyFaults()
+  {
+    motor.clearStickyFaults();
+  }
+
+  /**
+   * Set the absolute encoder to be a compatible absolute encoder.
+   *
+   * @param encoder The encoder to use.
+   */
+  @Override
+  public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
+  {
+    // Do not support.
+    return this;
+  }
+
+  /**
+   * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
+   *
+   * @param positionConversionFactor The conversion factor to apply for position.
+   *                                 <p><br>
+   *                                 Degrees: <br>
+   *                                 <code>
+   *                                 360 / (angleGearRatio * encoderTicksPerRotation)
+   *                                 </code><br>
+   *                                 <p><br>
+   *                                 Meters:<br>
+   *                                 <code>
+   *                                 (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation)
+   *                                 </code>
+   */
+  @Override
+  public void configureIntegratedEncoder(double positionConversionFactor)
+  {
+    cfg.refresh(configuration);
+
+    positionConversionFactor = 1 / positionConversionFactor;
+    if (!isDriveMotor)
+    {
+      positionConversionFactor *= 360;
+    }
+    conversionFactor = positionConversionFactor;
+
+    configuration.MotionMagic =
+        configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor)
+                                 .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100)
+                                 .withMotionMagicExpo_kV(0.12 * positionConversionFactor)
+                                 .withMotionMagicExpo_kA(0.1);
+
+    /*
+    configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor)
+                          .withSensorToMechanismRatio(positionConversionFactor);
+     */
+
+    cfg.apply(configuration);
+  }
+
+  /**
+   * Configure the PIDF values for the closed loop controller. 0 is disabled or off.
+   *
+   * @param config Configuration class holding the PIDF values.
+   */
+  @Override
+  public void configurePIDF(PIDFConfig config)
+  {
+
+    cfg.refresh(configuration.Slot0);
+    cfg.apply(
+        configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f));
+    //    configuration.slot0.integralZone = config.iz;
+    //    configuration.slot0.closedLoopPeakOutput = config.output.max;
+  }
+
+  /**
+   * Configure the PID wrapping for the position closed loop controller.
+   *
+   * @param minInput Minimum PID input.
+   * @param maxInput Maximum PID input.
+   */
+  @Override
+  public void configurePIDWrapping(double minInput, double maxInput)
+  {
+    cfg.refresh(configuration.ClosedLoopGeneral);
+    configuration.ClosedLoopGeneral.ContinuousWrap = true;
+    cfg.apply(configuration.ClosedLoopGeneral);
+  }
+
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  @Override
+  public void disablePIDWrapping()
+  {
+    cfg.refresh(configuration.ClosedLoopGeneral);
+    configuration.ClosedLoopGeneral.ContinuousWrap = false;
+    cfg.apply(configuration.ClosedLoopGeneral);
+  }
+
+  /**
+   * Set the idle mode.
+   *
+   * @param isBrakeMode Set the brake mode.
+   */
+  @Override
+  public void setMotorBrake(boolean isBrakeMode)
+  {
+    motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+  }
+
+  /**
+   * Set the motor to be inverted.
+   *
+   * @param inverted State of inversion.
+   */
+  @Override
+  public void setInverted(boolean inverted)
+  {
+    //    Timer.delay(1);
+    cfg.refresh(configuration.MotorOutput);
+    configuration.MotorOutput.withInverted(
+        inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive);
+    cfg.apply(configuration.MotorOutput);
+  }
+
+  /**
+   * Save the configurations from flash to EEPROM.
+   */
+  @Override
+  public void burnFlash()
+  {
+    // Do nothing
+  }
+
+  /**
+   * Set the percentage output.
+   *
+   * @param percentOutput percent out for the motor controller.
+   */
+  @Override
+  public void set(double percentOutput)
+  {
+    motor.set(percentOutput);
+  }
+
+  /**
+   * Set the closed loop PID controller reference point.
+   *
+   * @param setpoint    Setpoint in MPS or Angle in degrees.
+   * @param feedforward Feedforward in volt-meter-per-second or kV.
+   */
+  @Override
+  public void setReference(double setpoint, double feedforward)
+  {
+    setReference(setpoint, feedforward, getPosition());
+  }
+
+  /**
+   * Set the closed loop PID controller reference point.
+   *
+   * @param setpoint    Setpoint in meters per second or angle in degrees.
+   * @param feedforward Feedforward in volt-meter-per-second or kV.
+   * @param position    Only used on the angle motor, the position of the motor in degrees.
+   */
+  @Override
+  public void setReference(double setpoint, double feedforward, double position)
+  {
+    //    if (SwerveDriveTelemetry.isSimulation)
+    //    {
+    //      PhysicsSim.getInstance().run();
+    //    }
+
+    if (isDriveMotor)
+    {
+      motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward));
+    } else
+    {
+      motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0));
+    }
+  }
+
+  /**
+   * Get the voltage output of the motor controller.
+   *
+   * @return Voltage output.
+   */
+  @Override
+  public double getVoltage()
+  {
+    return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(Volts);
+  }
+
+  /**
+   * Set the voltage of the motor.
+   *
+   * @param voltage Voltage to set.
+   */
+  @Override
+  public void setVoltage(double voltage)
+  {
+    motor.setVoltage(voltage);
+  }
+
+  /**
+   * Get the applied dutycycle output.
+   *
+   * @return Applied dutycycle output to the motor.
+   */
+  @Override
+  public double getAppliedOutput()
+  {
+    return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
+  }
+
+  /**
+   * Get the velocity of the integrated encoder.
+   *
+   * @return velocity in Meters Per Second, or Degrees per Second.
+   */
+  @Override
+  public double getVelocity()
+  {
+    return motor.getVelocity().getValue().magnitude();
+  }
+
+  /**
+   * Get the position of the integrated encoder.
+   *
+   * @return Position in Meters or Degrees.
+   */
+  @Override
+  public double getPosition()
+  {
+    return motor.getPosition().getValue().magnitude();
+  }
+
+  /**
+   * Set the integrated encoder position.
+   *
+   * @param position Integrated encoder position. Should be angle in degrees or meters.
+   */
+  @Override
+  public void setPosition(double position)
+  {
+    if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
+    {
+      cfg.setPosition(Degrees.of(position).in(Rotations));
+    }
+  }
+
+  /**
+   * Set the voltage compensation for the swerve module motor.
+   *
+   * @param nominalVoltage Nominal voltage for operation to output to.
+   */
+  @Override
+  public void setVoltageCompensation(double nominalVoltage)
+  {
+    // Do not implement
+  }
+
+  /**
+   * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+   * voltage compensation. This is useful to protect the motor from current spikes.
+   *
+   * @param currentLimit Current limit in AMPS at free speed.
+   */
+  @Override
+  public void setCurrentLimit(int currentLimit)
+  {
+    cfg.refresh(configuration.CurrentLimits);
+    cfg.apply(
+        configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit)
+                                   .withSupplyCurrentLimitEnable(true));
+  }
+
+  /**
+   * Set the maximum rate the open/closed loop output can change by.
+   *
+   * @param rampRate Time in seconds to go from 0 to full throttle.
+   */
+  @Override
+  public void setLoopRampRate(double rampRate)
+  {
+    cfg.refresh(configuration.ClosedLoopRamps);
+    cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate));
+  }
+
+  /**
+   * Get the motor object from the module.
+   *
+   * @return Motor object.
+   */
+  @Override
+  public Object getMotor()
+  {
+    return motor;
+  }
+
+  /**
+   * Get the {@link DCMotor} of the motor class.
+   *
+   * @return {@link DCMotor} of this type.
+   */
+  @Override
+  public DCMotor getSimMotor()
+  {
+    if (simMotor == null)
+    {
+      simMotor = DCMotor.getKrakenX60(1);
+    }
+    return simMotor;
+  }
+
+  /**
+   * Queries whether the absolute encoder is directly attached to the motor controller.
+   *
+   * @return connected absolute encoder state.
+   */
+  @Override
+  public boolean usingExternalFeedbackSensor()
+  {
+    return absoluteEncoder;
+  }
+
+  /**
+   * Closes handles for unit testing.
+   */
+  @Override
+  public void close()
+  {
+    motor.close();
+  }
+}

swervelib/motors/TalonFXSwerve.java

@@ -131,6 +131,12 @@ public class TalonFXSwerve extends SwerveMotor
     }
   }
 
+  @Override
+  public void close() {
+    motor.close();
+  }
+
+
   /**
    * Clear the sticky faults on the motor controller.
    */
@@ -221,6 +227,17 @@ public class TalonFXSwerve extends SwerveMotor
     cfg.apply(configuration.ClosedLoopGeneral);
   }
 
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  @Override
+  public void disablePIDWrapping()
+  {
+    cfg.refresh(configuration.ClosedLoopGeneral);
+    configuration.ClosedLoopGeneral.ContinuousWrap = false;
+    cfg.apply(configuration.ClosedLoopGeneral);
+  }
+
   /**
    * Set the idle mode.
    *
@@ -442,7 +459,7 @@ public class TalonFXSwerve extends SwerveMotor
    * @return connected absolute encoder state.
    */
   @Override
-  public boolean isAttachedAbsoluteEncoder()
+  public boolean usingExternalFeedbackSensor()
   {
     return absoluteEncoder;
   }

swervelib/motors/TalonSRXSwerve.java

@@ -9,6 +9,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import edu.wpi.first.math.system.plant.DCMotor;
 import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.encoders.TalonSRXEncoderSwerve;
 import swervelib.math.SwerveMath;
 import swervelib.parser.PIDFConfig;
 import swervelib.parser.json.modules.ConversionFactorsJson;
@@ -31,7 +32,7 @@ public class TalonSRXSwerve extends SwerveMotor
   /**
    * Whether the absolute encoder is integrated.
    */
-  private final boolean               absoluteEncoder          = false;
+  private       boolean               absoluteEncoder          = false;
   /**
    * TalonSRX motor controller.
    */
@@ -84,6 +85,11 @@ public class TalonSRXSwerve extends SwerveMotor
     this(new WPI_TalonSRX(id), isDriveMotor, motorType);
   }
 
+  @Override
+  public void close() {
+    motor.close();
+  }
+
   /**
    * Configure the factory defaults.
    */
@@ -114,6 +120,7 @@ public class TalonSRXSwerve extends SwerveMotor
   @Override
   public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
   {
+    absoluteEncoder = encoder instanceof TalonSRXEncoderSwerve;
     // Do not support.
     return this;
   }
@@ -217,6 +224,15 @@ public class TalonSRXSwerve extends SwerveMotor
     // Do nothing
   }
 
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  @Override
+  public void disablePIDWrapping()
+  {
+    // Do nothing
+  }
+
   /**
    * Set the idle mode.
    *
@@ -468,7 +484,7 @@ public class TalonSRXSwerve extends SwerveMotor
    * @return connected absolute encoder state.
    */
   @Override
-  public boolean isAttachedAbsoluteEncoder()
+  public boolean usingExternalFeedbackSensor()
   {
     return absoluteEncoder;
   }

swervelib/motors/ThriftyNovaSwerve.java

@@ -26,6 +26,14 @@ import swervelib.parser.PIDFConfig;
 public class ThriftyNovaSwerve extends SwerveMotor
 {
 
+  /**
+   * {@link DCMotor} for simulation and calculations.
+   */
+  private final DCMotor       simMotor;
+  /**
+   * Closed-loop PID controller.
+   */
+  public        PIDController pid;
   /**
    * ThriftyNova Instance.
    */
@@ -34,10 +42,6 @@ public class ThriftyNovaSwerve extends SwerveMotor
    * The Encoder type being used
    */
   private       EncoderType   encoderType;
-  /**
-   * Closed-loop PID controller.
-   */
-  public        PIDController pid;
   /**
    * Factory default already occurred.
    */
@@ -58,10 +62,6 @@ public class ThriftyNovaSwerve extends SwerveMotor
    * The position conversion factor for the encoder
    */
   private       double        velocityConversionFactor = 1.0 / 60.0;
-  /**
-   * {@link DCMotor} for simulation and calculations.
-   */
-  private final DCMotor       simMotor;
 
   /**
    * Initialize the swerve motor.
@@ -105,6 +105,16 @@ public class ThriftyNovaSwerve extends SwerveMotor
     this(new ThriftyNova(id), isDriveMotor, motor);
   }
 
+  @Override
+  public void close() {
+    try {
+    motor.close();
+    } catch (Exception e) {
+      throw new RuntimeException(e);
+    }
+  }
+
+
   /**
    * Set factory defaults on the motor controller.
    */
@@ -226,6 +236,15 @@ public class ThriftyNovaSwerve extends SwerveMotor
     // Do nothing
   }
 
+  /**
+   * Disable PID Wrapping on the motor.
+   */
+  @Override
+  public void disablePIDWrapping()
+  {
+    // Do nothing
+  }
+
   /**
    * Set the idle mode.
    *
@@ -416,7 +435,7 @@ public class ThriftyNovaSwerve extends SwerveMotor
    * @return connected absolute encoder state.
    */
   @Override
-  public boolean isAttachedAbsoluteEncoder()
+  public boolean usingExternalFeedbackSensor()
   {
     return EncoderType.ABS == encoderType;
   }

swervelib/parser/PIDFConfig.java

@@ -103,6 +103,10 @@ public class PIDFConfig
    */
   public PIDController createPIDController()
   {
-    return new PIDController(p, i, d);
+    PIDController pidController = new PIDController(p, i, d);
+    if (iz != 0) {
+      pidController.setIZone(iz);
+    }
+    return pidController;
   }
 }

swervelib/parser/SwerveParser.java

@@ -1,5 +1,6 @@
 package swervelib.parser;
 
+import com.fasterxml.jackson.databind.DeserializationFeature;
 import com.fasterxml.jackson.databind.JsonNode;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -57,17 +58,21 @@ public class SwerveParser
     checkDirectory(directory);
     swerveDriveJson =
         new ObjectMapper()
+            .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
             .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
     controllerPropertiesJson =
         new ObjectMapper()
+            .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
             .readValue(
                 new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class);
     pidfPropertiesJson =
         new ObjectMapper()
+            .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
             .readValue(
                 new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
     physicalPropertiesJson =
         new ObjectMapper()
+            .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
             .readValue(
                 new File(directory, "modules/physicalproperties.json"),
                 PhysicalPropertiesJson.class);
@@ -77,7 +82,9 @@ public class SwerveParser
       moduleConfigs.put(swerveDriveJson.modules[i], i);
       File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]);
       assert moduleFile.exists();
-      moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class);
+      moduleJsons[i] = new ObjectMapper()
+          .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
+          .readValue(moduleFile, ModuleJson.class);
     }
   }
 

swervelib/parser/json/DeviceJson.java

@@ -6,13 +6,13 @@ import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
 
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.studica.frc.AHRS.NavXComType;
-
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.DriverStation;
 import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
 import swervelib.encoders.CANCoderSwerve;
 import swervelib.encoders.CanAndMagSwerve;
-import swervelib.encoders.PWMDutyCycleEncoderSwerve;
+import swervelib.encoders.DIODutyCycleEncoderSwerve;
+import swervelib.encoders.SparkFlexEncoderSwerve;
 import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
 import swervelib.encoders.SparkMaxEncoderSwerve;
 import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -26,12 +26,14 @@ import swervelib.imu.CanandgyroSwerve;
 import swervelib.imu.NavXSwerve;
 import swervelib.imu.Pigeon2Swerve;
 import swervelib.imu.PigeonSwerve;
+import swervelib.imu.PigeonViaTalonSRXSwerve;
 import swervelib.imu.SwerveIMU;
 import swervelib.motors.SparkFlexSwerve;
 import swervelib.motors.SparkMaxBrushedMotorSwerve;
 import swervelib.motors.SparkMaxBrushedMotorSwerve.Type;
 import swervelib.motors.SparkMaxSwerve;
 import swervelib.motors.SwerveMotor;
+import swervelib.motors.TalonFXSSwerve;
 import swervelib.motors.TalonFXSwerve;
 import swervelib.motors.TalonSRXSwerve;
 import swervelib.motors.ThriftyNovaSwerve;
@@ -81,6 +83,11 @@ public class DeviceJson
         return new SparkMaxAnalogEncoderSwerve(motor, 3.3);
       case "sparkmax_analog5v":
         return new SparkMaxAnalogEncoderSwerve(motor, 5);
+      case "sparkflex_integrated":
+      case "sparkflex_attached":
+      case "sparkflex_canandmag":
+      case "sparkflex_canandcoder":
+        return new SparkFlexEncoderSwerve(motor, 360);
       case "canandcoder_can":
       case "canandmag_can":
         return new CanAndMagSwerve(id);
@@ -89,13 +96,16 @@ public class DeviceJson
       case "throughbore":
       case "am_mag":
       case "dutycycle":
-        return new PWMDutyCycleEncoderSwerve(id);
+        return new DIODutyCycleEncoderSwerve(id);
       case "thrifty":
       case "ma3":
       case "analog":
         return new AnalogAbsoluteEncoderSwerve(id);
       case "cancoder":
         return new CANCoderSwerve(id, canbus != null ? canbus : "");
+      case "srxmag_standalone":
+        return new TalonSRXEncoderSwerve(new TalonSRXSwerve(id, false, DCMotor.getCIM(1)),
+                                         FeedbackDevice.PulseWidthEncodedPosition);
       case "talonsrx_pwm":
         return new TalonSRXEncoderSwerve(motor, FeedbackDevice.PulseWidthEncodedPosition);
       case "talonsrx_analog":
@@ -151,6 +161,8 @@ public class DeviceJson
         return new NavXSwerve(NavXComType.kMXP_UART);
       case "pigeon":
         return new PigeonSwerve(id);
+      case "pigeon_via_talonsrx":
+        return new PigeonViaTalonSRXSwerve(id);
       case "pigeon2":
         return new Pigeon2Swerve(id, canbus != null ? canbus : "");
       default:
@@ -172,10 +184,22 @@ public class DeviceJson
     }
     switch (type)
     {
+      case "talonfxs_neo":
+        return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNEO(1));
+      case "talonfxs_neo550":
+        return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeo550(1));
+      case "talonfxs_vortex":
+        return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1));
+      case "talonfxs_minion":
+        throw new UnsupportedOperationException("Cannot create minion combination yet"); //new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1));
       case "sparkmax_neo":
       case "neo":
       case "sparkmax":
         return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNEO(1));
+      case "sparkmax_vortex":
+        return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
+      case "sparkmax_minion":
+        throw new UnsupportedOperationException("Cannot create minion combination yet");
       case "sparkmax_neo550":
       case "neo550":
         return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
@@ -187,6 +211,8 @@ public class DeviceJson
         return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNEO(1));
       case "sparkflex_neo550":
         return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
+      case "sparkflex_minion":
+        throw new UnsupportedOperationException("Cannot create minion combination yet");
       case "falcon500":
       case "falcon":
         return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getFalcon500(1));
@@ -230,6 +256,10 @@ public class DeviceJson
         return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNEO(1));
       case "nova_neo550":
         return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
+      case "nova_vortex":
+        return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
+      case "nova_minion":
+        throw new UnsupportedOperationException("Cannot create minion combination");//return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getMinion(1));
       default:
         throw new RuntimeException(type + " is not a recognized motor type.");
     }

swervelib/parser/json/ModuleJson.java

@@ -75,6 +75,14 @@ public class ModuleJson
     SwerveMotor           angleMotor = angle.createMotor(false);
     SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
 
+    //Throw an error if module locations are improperly set
+    if (location.front == 0 && location.left == 0)
+    {
+      throw new RuntimeException("Improper Module Location Settings!\n" +
+                                 "Your module location is set to 0 for both 'front' and 'left' values.\n" +
+                                 "Set the distance from the center of the robot to the center of the wheel in your module JSON file!");
+    }
+
     // Set the conversion factors to null if they are both 0.
     if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null)
     {
@@ -119,8 +127,8 @@ public class ModuleJson
         conversionFactors,
         absEncoder,
         absoluteEncoderOffset,
-        Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x),
+        Units.inchesToMeters(Math.round(location.front)),
-        Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y),
+        Units.inchesToMeters(Math.round(location.left)),
         anglePIDF,
         velocityPIDF,
         physicalCharacteristics,

swervelib/parser/json/PhysicalPropertiesJson.java

@@ -63,12 +63,11 @@ public class PhysicalPropertiesJson
     if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() &&
         conversionFactors.isAngleEmpty())
     {
-      new Alert("Configuration",
+      throw new RuntimeException(
                 "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
                 "} \nis deprecated, please use\n" +
                 "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
-                conversionFactor.angle + "} }",
+                conversionFactor.angle + "} }");
-                AlertType.kError).set(true);
     }
 
     return new SwerveModulePhysicalCharacteristics(

swervelib/parser/json/modules/LocationJson.java

@@ -10,9 +10,9 @@ public class LocationJson
   /**
    * Location of the swerve module in inches from the center of the robot horizontally.
    */
-  public double front = 0, x = 0;
+  public double front = 0;
   /**
    * Location of the swerve module in inches from the center of the robot vertically.
    */
-  public double left = 0, y = 0;
+  public double left = 0;
 }

swervelib/simulation/SwerveModuleSimulation.java

@@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation;
+import swervelib.SwerveDrive;
 import swervelib.parser.SwerveModulePhysicalCharacteristics;
 
 /**
@@ -14,12 +15,16 @@ import swervelib.parser.SwerveModulePhysicalCharacteristics;
 public class SwerveModuleSimulation
 {
 
-  private SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null;
+  /**
+   * MapleSim module.
+   */
+  public SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null;
 
   /**
    * Configure the maple sim module
    *
-   * @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation
+   * @param simModule               the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for
+   *                                simulation
    * @param physicalCharacteristics Physical characteristics of the swerve drive from the JSON or built.
    */
   public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation simModule,
@@ -42,6 +47,31 @@ public class SwerveModuleSimulation
     mapleSimModule.runModuleState(desiredState);
   }
 
+  /**
+   * Runs a drive motor characterization on the sim module. This is called from
+   * {@link swervelib.SwerveDriveTest#runDriveMotorsCharacterizationOnSimModules(SwerveDrive, double, boolean)}  to run
+   * sysId during simulation
+   *
+   * @param desiredFacing the desired facing of the module
+   * @param volts         the voltage to run
+   */
+  public void runDriveMotorCharacterization(Rotation2d desiredFacing, double volts)
+  {
+    mapleSimModule.runDriveMotorCharacterization(desiredFacing, volts);
+  }
+
+  /**
+   * Runs a drive motor characterization on the sim module. This method is called from
+   * {@link swervelib.SwerveDriveTest#runAngleMotorsCharacterizationOnSimModules(SwerveDrive, double)} to run sysId
+   * during simulation
+   *
+   * @param volts the voltage to run
+   */
+  public void runAngleMotorCharacterization(double volts)
+  {
+    mapleSimModule.runSteerMotorCharacterization(volts);
+  }
+
   /**
    * Get the simulated swerve module position.
    *

swervelib/telemetry/SwerveDriveTelemetry.java

@@ -429,7 +429,7 @@ public class SwerveDriveTelemetry
       updateSwerveTelemetrySettings();
     }
     measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond;
-    measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond;
+    measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vyMetersPerSecond;
     measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond);
 
     desiredChassisSpeeds[0] = desiredChassisSpeedsObj.vxMetersPerSecond;