Update to 2024.4.8

swervelib/SwerveDrive.java

@@ -23,7 +23,9 @@ import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
+import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
@@ -32,6 +34,7 @@ import swervelib.imu.Pigeon2Swerve;
 import swervelib.imu.SwerveIMU;
 import swervelib.math.SwerveMath;
 import swervelib.motors.TalonFXSwerve;
+import swervelib.parser.Cache;
 import swervelib.parser.SwerveControllerConfiguration;
 import swervelib.parser.SwerveDriveConfiguration;
 import swervelib.simulation.SwerveIMUSimulation;
@@ -58,6 +61,10 @@ public class SwerveDrive
    * Swerve odometry.
    */
   public final  SwerveDrivePoseEstimator swerveDrivePoseEstimator;
+  /**
+   * IMU reading cache for robot readings.
+   */
+  public final  Cache<Rotation3d>        imuReadingCache;
   /**
    * Swerve modules.
    */
@@ -155,10 +162,12 @@ public class SwerveDrive
     if (SwerveDriveTelemetry.isSimulation)
     {
       simIMU = new SwerveIMUSimulation();
+      imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
     } else
     {
       imu = config.imu;
       imu.factoryDefault();
+      imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
     }
 
     this.swerveModules = config.modules;
@@ -213,6 +222,24 @@ public class SwerveDrive
     checkIfTunerXCompatible();
   }
 
+  /**
+   * Update the cache validity period for the robot.
+   *
+   * @param imu             IMU reading cache validity period in milliseconds.
+   * @param driveMotor      Drive motor reading cache in milliseconds.
+   * @param absoluteEncoder Absolute encoder reading cache in milliseconds.
+   */
+  public void updateCacheValidityPeriods(long imu, long driveMotor, long absoluteEncoder)
+  {
+    imuReadingCache.updateValidityPeriod(imu);
+    for (SwerveModule module : swerveModules)
+    {
+      module.drivePositionCache.updateValidityPeriod(driveMotor);
+      module.driveVelocityCache.updateValidityPeriod(driveMotor);
+      module.absolutePositionCache.updateValidityPeriod(absoluteEncoder);
+    }
+  }
+
   /**
    * Check all components to ensure that Tuner X Swerve Generator is recommended instead.
    */
@@ -623,7 +650,7 @@ public class SwerveDrive
     odometryLock.lock();
     swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
     odometryLock.unlock();
-    kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation()));
+    kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw()));
   }
 
   /**
@@ -695,6 +722,7 @@ public class SwerveDrive
     {
       setGyroOffset(imu.getRawRotation3d().minus(gyro));
     }
+    imuReadingCache.update();
   }
 
   /**
@@ -704,13 +732,14 @@ public class SwerveDrive
   {
     // Resets the real gyro or the angle accumulator, depending on whether the robot is being
     // simulated
-    if (!SwerveDriveTelemetry.isSimulation)
-    {
-      imu.setOffset(imu.getRawRotation3d());
-    } else
+    if (SwerveDriveTelemetry.isSimulation)
     {
       simIMU.setAngle(0);
+    } else
+    {
+      setGyroOffset(imu.getRawRotation3d());
     }
+    imuReadingCache.update();
     swerveController.lastAngleScalar = 0;
     lastHeadingRadians = 0;
     resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d()));
@@ -724,13 +753,7 @@ public class SwerveDrive
   public Rotation2d getYaw()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (!SwerveDriveTelemetry.isSimulation)
-    {
-      return Rotation2d.fromRadians(imu.getRotation3d().getZ());
-    } else
-    {
-      return simIMU.getYaw();
-    }
+    return Rotation2d.fromRadians(imuReadingCache.getValue().getZ());
   }
 
   /**
@@ -741,13 +764,7 @@ public class SwerveDrive
   public Rotation2d getPitch()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (!SwerveDriveTelemetry.isSimulation)
-    {
-      return Rotation2d.fromRadians(imu.getRotation3d().getY());
-    } else
-    {
-      return simIMU.getPitch();
-    }
+    return Rotation2d.fromRadians(imuReadingCache.getValue().getY());
   }
 
   /**
@@ -758,13 +775,7 @@ public class SwerveDrive
   public Rotation2d getRoll()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (!SwerveDriveTelemetry.isSimulation)
-    {
-      return Rotation2d.fromRadians(imu.getRotation3d().getX());
-    } else
-    {
-      return simIMU.getRoll();
-    }
+    return Rotation2d.fromRadians(imuReadingCache.getValue().getX());
   }
 
   /**
@@ -775,13 +786,7 @@ public class SwerveDrive
   public Rotation3d getGyroRotation3d()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (!SwerveDriveTelemetry.isSimulation)
-    {
-      return imu.getRotation3d();
-    } else
-    {
-      return simIMU.getGyroRotation3d();
-    }
+    return imuReadingCache.getValue();
   }
 
   /**
@@ -1016,6 +1021,7 @@ public class SwerveDrive
     {
       imu.setOffset(offset);
     }
+    imuReadingCache.update();
   }
 
   /**
@@ -1085,6 +1091,21 @@ public class SwerveDrive
     return swerveDriveConfiguration.modules;
   }
 
+  /**
+   * Get the {@link SwerveModule}'s as a {@link HashMap} where the key is the swerve module configuration name.
+   *
+   * @return {@link HashMap}(Module Name, SwerveModule)
+   */
+  public Map<String, SwerveModule> getModuleMap()
+  {
+    Map<String, SwerveModule> map = new HashMap<String, SwerveModule>();
+    for (SwerveModule module : swerveModules)
+    {
+      map.put(module.configuration.name, module);
+    }
+    return map;
+  }
+
   /**
    * Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in
    * autonomous.

swervelib/SwerveDriveTest.java

@@ -1,9 +1,31 @@
 package swervelib;
 
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+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.SwerveModuleState;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 
 /**
@@ -12,6 +34,27 @@ import swervelib.encoders.SwerveAbsoluteEncoder;
 public class SwerveDriveTest
 {
 
+  /**
+   * Tracks the voltage being applied to a motor
+   */
+  private static final MutableMeasure<Voltage>            m_appliedVoltage = mutable(Volts.of(0));
+  /**
+   * Tracks the distance travelled of a position motor
+   */
+  private static final MutableMeasure<Distance>           m_distance       = mutable(Meters.of(0));
+  /**
+   * Tracks the velocity of a positional motor
+   */
+  private static final MutableMeasure<Velocity<Distance>> m_velocity       = mutable(MetersPerSecond.of(0));
+  /**
+   * Tracks the rotations of an angular motor
+   */
+  private static final MutableMeasure<Angle>              m_anglePosition  = mutable(Degrees.of(0));
+  /**
+   * Tracks the velocity of an angular motor
+   */
+  private static final MutableMeasure<Velocity<Angle>>    m_angVelocity    = mutable(DegreesPerSecond.of(0));
+
   /**
    * Set the angle of the modules to a given {@link Rotation2d}
    *
@@ -22,7 +65,7 @@ public class SwerveDriveTest
   {
     for (SwerveModule swerveModule : swerveDrive.getModules())
     {
-      swerveModule.setDesiredState(new SwerveModuleState(0, moduleAngle), false, true);
+      swerveModule.getAngleMotor().setReference(moduleAngle.getDegrees(), 0);
     }
   }
 
@@ -46,7 +89,7 @@ public class SwerveDriveTest
    * @param swerveDrive {@link SwerveDrive} to control.
    * @param percentage  DutyCycle percentage to send to angle motors.
    */
-  public static void powerAngleMotors(SwerveDrive swerveDrive, double percentage)
+  public static void powerAngleMotorsDutyCycle(SwerveDrive swerveDrive, double percentage)
   {
     for (SwerveModule swerveModule : swerveDrive.getModules())
     {
@@ -195,4 +238,178 @@ public class SwerveDriveTest
     DriverStation.reportWarning("Average Coupling Ratio: " + (couplingRatioSum / 4.0), false);
     return (couplingRatioSum / 4.0);
   }
+
+  /**
+   * Creates a SysIdRoutine.Config with a custom final timeout
+   *
+   * @param timeout - the most a SysIdRoutine should run
+   * @return A custom SysIdRoutine.Config
+   */
+  public static Config createConfigCustomTimeout(double timeout)
+  {
+    return new Config(null, null, Seconds.of(timeout));
+  }
+
+  /**
+   * Logs output, position and velocuty info form the drive motor to the SysIdRoutineLog <br /> Although SysIdRoutine
+   * expects to be logging Voltage, this function logs in Duty-Cycle (percent output) because it results in correctly
+   * adjusted values in the analysis for use in this library.
+   *
+   * @param module - the swerve module being logged
+   * @param log    - the logger
+   */
+  public static void logDriveMotorDutyCycle(SwerveModule module, SysIdRoutineLog log)
+  {
+    logDriveMotorActivity(module, log, () -> module.getDriveMotor().getVoltage() / RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * Logs voltage, position and velocuty info form the drive motor to the SysIdRoutineLog
+   *
+   * @param module - the swerve module being logged
+   * @param log    - the logger
+   */
+  public static void logDriveMotorVoltage(SwerveModule module, SysIdRoutineLog log)
+  {
+    logDriveMotorActivity(module, log, () -> module.getDriveMotor().getVoltage());
+  }
+
+  /**
+   * Logs power, position and velocuty info form the drive motor to the SysIdRoutineLog
+   *
+   * @param module        - the swerve module being logged
+   * @param log           - the logger
+   * @param powerSupplied - a functional supplier of the power to be logged
+   */
+  public static void logDriveMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied)
+  {
+    double power    = powerSupplied.get();
+    double distance = module.getPosition().distanceMeters;
+    double velocity = module.getDriveMotor().getVelocity();
+    SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Power", power);
+    SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Position", distance);
+    SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Velocity", velocity);
+    log.motor("drive-" + module.configuration.name)
+       .voltage(m_appliedVoltage.mut_replace(power, Volts))
+       .linearPosition(m_distance.mut_replace(distance, Meters))
+       .linearVelocity(m_velocity.mut_replace(velocity, MetersPerSecond));
+  }
+
+  /**
+   * Sets up the SysId runner and logger for the drive motors
+   *
+   * @param config          - The SysIdRoutine.Config to use
+   * @param swerveSubsystem - the subsystem to add to requirements
+   * @param swerveDrive     - the SwerveDrive from which to access motor info
+   * @param maxVolts        - The maximum voltage that should be applied to the drive motors.
+   * @return A SysIdRoutine runner
+   */
+  public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swerveSubsystem,
+                                                  SwerveDrive swerveDrive, double maxVolts)
+  {
+    return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
+        (Measure<Voltage> voltage) -> {
+          SwerveDriveTest.centerModules(swerveDrive);
+          SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
+        },
+        log -> {
+          for (SwerveModule module : swerveDrive.getModules())
+          {
+            logDriveMotorVoltage(module, log);
+          }
+        }, swerveSubsystem));
+  }
+
+  /**
+   * Logs info about the angle motor to the SysIdRoutineLog. <br /> Although SysIdRoutine expects to be logging Voltage,
+   * this function logs in Duty-Cycle (percent output) because it results in correctly adjusted values in the analysis
+   * for use in this library.
+   *
+   * @param module - the swerve module being logged
+   * @param log    - the logger
+   */
+  public static void logAngularMotorDutyCycle(SwerveModule module, SysIdRoutineLog log)
+  {
+    logAngularMotorActivity(module,
+                            log,
+                            () -> module.getAngleMotor().getVoltage() / RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * Logs info about the angle motor to the SysIdRoutineLog
+   *
+   * @param module - the swerve module being logged
+   * @param log    - the logger
+   */
+  public static void logAngularMotorVoltage(SwerveModule module, SysIdRoutineLog log)
+  {
+    logAngularMotorActivity(module, log, () -> module.getAngleMotor().getVoltage());
+  }
+
+  /**
+   * Logs info about the angle motor to the SysIdRoutineLog
+   *
+   * @param module        - the swerve module being logged
+   * @param log           - the logger
+   * @param powerSupplied - a functional supplier of the power to be logged
+   */
+  public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied)
+  {
+    double power    = powerSupplied.get();
+    double angle    = module.getAbsolutePosition();
+    double velocity = module.getAbsoluteEncoder().getVelocity();
+    SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Power", power);
+    SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Position", angle);
+    SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Absolute Encoder Velocity", velocity);
+    log.motor("angle-" + module.configuration.name)
+       .voltage(m_appliedVoltage.mut_replace(power, Volts))
+       .angularPosition(m_anglePosition.mut_replace(angle, Degrees))
+       .angularVelocity(m_angVelocity.mut_replace(velocity, DegreesPerSecond));
+  }
+
+  /**
+   * Sets up the SysId runner and logger for the angle motors
+   *
+   * @param config          - The SysIdRoutine.Config to use
+   * @param swerveSubsystem - the subsystem to add to requirements
+   * @param swerveDrive     - the SwerveDrive from which to access motor info
+   * @return A SysIdRoutineRunner
+   */
+  public static SysIdRoutine setAngleSysIdRoutine(Config config, SubsystemBase swerveSubsystem,
+                                                  SwerveDrive swerveDrive)
+  {
+    return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
+        (Measure<Voltage> voltage) -> {
+          SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
+          SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
+        },
+        log -> {
+          for (SwerveModule module : swerveDrive.getModules())
+          {
+            logAngularMotorVoltage(module, log);
+          }
+        }, swerveSubsystem));
+  }
+
+  /**
+   * Creates a command that can be mapped to a button or other trigger. Delays can be set to customize the length of
+   * each part of the SysId Routine
+   *
+   * @param sysIdRoutine   - The Sys Id routine runner
+   * @param delay          - seconds between each portion to allow motors to spin down, etc...
+   * @param quasiTimeout   - seconds to run the Quasistatic routines, so robot doesn't get too far
+   * @param dynamicTimeout - seconds to run the Dynamic routines, 2-3 secs should be enough
+   * @return A command that can be mapped to a button or other trigger
+   */
+  public static Command generateSysIdCommand(SysIdRoutine sysIdRoutine, double delay, double quasiTimeout,
+                                             double dynamicTimeout)
+  {
+    return sysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward).withTimeout(quasiTimeout)
+                       .andThen(Commands.waitSeconds(delay))
+                       .andThen(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(quasiTimeout))
+                       .andThen(Commands.waitSeconds(delay))
+                       .andThen(sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout))
+                       .andThen(Commands.waitSeconds(delay))
+                       .andThen(sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout));
+  }
 }

swervelib/SwerveModule.java

@@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.math.SwerveMath;
 import swervelib.motors.SwerveMotor;
+import swervelib.parser.Cache;
 import swervelib.parser.SwerveModuleConfiguration;
 import swervelib.simulation.SwerveModuleSimulation;
 import swervelib.telemetry.Alert;
@@ -25,6 +26,18 @@ public class SwerveModule
    * Swerve module configuration options.
    */
   public final  SwerveModuleConfiguration configuration;
+  /**
+   * Absolute encoder position cache.
+   */
+  public final  Cache<Double>          absolutePositionCache;
+  /**
+   * Drive motor position cache.
+   */
+  public final  Cache<Double>          drivePositionCache;
+  /**
+   * Drive motor velocity cache.
+   */
+  public final  Cache<Double>          driveVelocityCache;
   /**
    * Swerve Motors.
    */
@@ -70,6 +83,7 @@ public class SwerveModule
    */
   private       boolean                synchronizeEncoderQueued = false;
 
+
   /**
    * Construct the swerve module and initialize the swerve module motors and absolute encoder.
    *
@@ -114,6 +128,9 @@ public class SwerveModule
       absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
     }
 
+    // Setup the cache for the absolute encoder position.
+    absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15);
+
     // Config angle motor/controller
     angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle);
     angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
@@ -136,11 +153,20 @@ public class SwerveModule
     driveMotor.burnFlash();
     angleMotor.burnFlash();
 
+    drivePositionCache = new Cache<>(driveMotor::getPosition, 15);
+    driveVelocityCache = new Cache<>(driveMotor::getVelocity, 15);
+
     if (SwerveDriveTelemetry.isSimulation)
     {
       simModule = new SwerveModuleSimulation();
     }
 
+    // Force a cache update on init.
+    driveVelocityCache.update();
+    drivePositionCache.update();
+    absolutePositionCache.update();
+
+    // Save the current state.
     lastState = getState();
 
     noEncoderWarning = new Alert("Motors",
@@ -198,8 +224,9 @@ public class SwerveModule
   {
     desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
     // Cosine compensation.
-    double velocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState)
-                                                         : desiredState.speedMetersPerSecond;
+    double velocity = configuration.useCosineCompensator
+                      ? getCosineCompensatedVelocity(desiredState)
+                      : desiredState.speedMetersPerSecond;
 
     if (isOpenLoop)
     {
@@ -298,7 +325,7 @@ public class SwerveModule
     Rotation2d azimuth;
     if (!SwerveDriveTelemetry.isSimulation)
     {
-      velocity = driveMotor.getVelocity();
+      velocity = driveVelocityCache.getValue();
       azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
     } else
     {
@@ -318,7 +345,7 @@ public class SwerveModule
     Rotation2d azimuth;
     if (!SwerveDriveTelemetry.isSimulation)
     {
-      position = driveMotor.getPosition();
+      position = drivePositionCache.getValue();
       azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
     } else
     {
@@ -333,6 +360,16 @@ public class SwerveModule
    * @return Absolute encoder angle in degrees in the range [0, 360).
    */
   public double getAbsolutePosition()
+  {
+    return absolutePositionCache.getValue();
+  }
+
+  /**
+   * Get the absolute position. Falls back to relative position on reading failure.
+   *
+   * @return Absolute encoder angle in degrees in the range [0, 360).
+   */
+  public double getRawAbsolutePosition()
   {
     double angle;
     if (absoluteEncoder != null)

swervelib/encoders/CANCoderSwerve.java

@@ -17,26 +17,30 @@ import swervelib.telemetry.Alert;
 public class CANCoderSwerve extends SwerveAbsoluteEncoder
 {
 
+  /**
+   * Wait time for status frames to show up.
+   */
+  private final double   STATUS_TIMEOUT_SECONDS = 0.02;
   /**
    * CANCoder with WPILib sendable and support.
    */
-  public  CANcoder encoder;
+  public        CANcoder encoder;
   /**
    * An {@link Alert} for if the CANCoder magnet field is less than ideal.
    */
-  private Alert    magnetFieldLessThanIdeal;
+  private       Alert    magnetFieldLessThanIdeal;
   /**
    * An {@link Alert} for if the CANCoder reading is faulty.
    */
-  private Alert    readingFaulty;
+  private       Alert    readingFaulty;
   /**
    * An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored.
    */
-  private Alert    readingIgnored;
+  private       Alert    readingIgnored;
   /**
    * An {@link Alert} for if the absolute encoder offset cannot be set.
    */
-  private Alert    cannotSetOffset;
+  private       Alert    cannotSetOffset;
 
   /**
    * Initialize the CANCoder on the standard CANBus.
@@ -45,7 +49,8 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
    */
   public CANCoderSwerve(int id)
   {
-    encoder = new CANcoder(id);
+    // Empty string uses the default canbus for the system
+    this(id, "");
   }
 
   /**
@@ -134,7 +139,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
       readingFaulty.set(false);
     }
 
-    StatusSignal<Double> angle = encoder.getAbsolutePosition().refresh();
+    StatusSignal<Double> angle = encoder.getAbsolutePosition();
 
     // 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
@@ -144,7 +149,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
       {
         break;
       }
-      angle = angle.waitForUpdate(0.01);
+      angle = angle.waitForUpdate(STATUS_TIMEOUT_SECONDS);
     }
     if (angle.getStatus() != StatusCode.OK)
     {

swervelib/imu/Pigeon2Swerve.java

@@ -16,6 +16,10 @@ import java.util.Optional;
 public class Pigeon2Swerve extends SwerveIMU
 {
 
+  /**
+   * Wait time for status frames to show up.
+   */
+  private final double     STATUS_TIMEOUT_SECONDS = 0.02;
   /**
    * Pigeon2 IMU device.
    */
@@ -23,11 +27,11 @@ public class Pigeon2Swerve extends SwerveIMU
   /**
    * Offset for the Pigeon 2.
    */
-  private Rotation3d offset      = new Rotation3d();
+  private       Rotation3d offset                 = new Rotation3d();
   /**
    * Inversion for the gyro
    */
-  private boolean    invertedIMU = false;
+  private       boolean    invertedIMU            = false;
 
   /**
    * Generate the SwerveIMU for pigeon.
@@ -106,10 +110,10 @@ public class Pigeon2Swerve extends SwerveIMU
     StatusSignal<Double> x = imu.getQuatX();
     StatusSignal<Double> y = imu.getQuatY();
     StatusSignal<Double> z = imu.getQuatZ();
-    Rotation3d reading = new Rotation3d(new Quaternion(w.refresh().getValue(),
-                                                       x.refresh().getValue(),
-                                                       y.refresh().getValue(),
-                                                       z.refresh().getValue()));
+    Rotation3d reading = new Rotation3d(new Quaternion(w.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+                                                       x.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+                                                       y.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+                                                       z.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()));
     return invertedIMU ? reading.unaryMinus() : reading;
   }
 
@@ -138,9 +142,9 @@ public class Pigeon2Swerve extends SwerveIMU
     StatusSignal<Double> yAcc = imu.getAccelerationY();
     StatusSignal<Double> zAcc = imu.getAccelerationZ();
 
-    return Optional.of(new Translation3d(xAcc.refresh().getValue(),
-                                         yAcc.refresh().getValue(),
-                                         zAcc.refresh().getValue()).times(9.81 / 16384.0));
+    return Optional.of(new Translation3d(xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+                                         yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+                                         zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()).times(9.81 / 16384.0));
   }
 
   /**

swervelib/motors/SparkMaxSwerve.java

@@ -43,6 +43,14 @@ public class SparkMaxSwerve extends SwerveMotor
    * Factory default already occurred.
    */
   private boolean               factoryDefaultOccurred = false;
+  /**
+   * Supplier for the velocity of the motor controller.
+   */
+  private Supplier<Double>      velocity;
+  /**
+   * Supplier for the position of the motor controller.
+   */
+  private Supplier<Double>      position;
 
   /**
    * Initialize the swerve motor.
@@ -62,6 +70,8 @@ public class SparkMaxSwerve extends SwerveMotor
     pid.setFeedbackDevice(
         encoder); // Configure feedback of the PID controller as the integrated encoder.
 
+    velocity = encoder::getVelocity;
+    position = encoder::getPosition;
     // Spin off configurations in a different thread.
     // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
   }
@@ -190,6 +200,8 @@ public class SparkMaxSwerve extends SwerveMotor
           false);
       absoluteEncoder = encoder;
       configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
+      velocity = absoluteEncoder::getVelocity;
+      position = absoluteEncoder::getAbsolutePosition;
     }
     return this;
   }
@@ -433,7 +445,7 @@ public class SparkMaxSwerve extends SwerveMotor
   @Override
   public double getVelocity()
   {
-    return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
+    return velocity.get();
   }
 
   /**
@@ -444,7 +456,7 @@ public class SparkMaxSwerve extends SwerveMotor
   @Override
   public double getPosition()
   {
-    return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition();
+    return position.get();
   }
 
   /**

swervelib/motors/TalonFXSwerve.java

@@ -33,6 +33,10 @@ public class TalonFXSwerve extends SwerveMotor
    * Velocity voltage setter for controlling drive motor.
    */
   private final VelocityVoltage    m_velocityVoltageSetter = new VelocityVoltage(0);
+  /**
+   * Wait time for status frames to show up.
+   */
+  private final double               STATUS_TIMEOUT_SECONDS = 0.02;
   /**
    * TalonFX motor controller.
    */
@@ -40,11 +44,12 @@ public class TalonFXSwerve extends SwerveMotor
   /**
    * Conversion factor for the motor.
    */
-  private double               conversionFactor;
+  private       double               conversionFactor;
   /**
    * Current TalonFX configuration.
    */
-  private TalonFXConfiguration configuration = new TalonFXConfiguration();
+  private       TalonFXConfiguration configuration          = new TalonFXConfiguration();
+
 
   /**
    * Constructor for TalonFX swerve motor.
@@ -351,7 +356,7 @@ public class TalonFXSwerve extends SwerveMotor
   @Override
   public double getVoltage()
   {
-    return motor.getMotorVoltage().refresh().getValue();
+    return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
   }
 
   /**
@@ -373,7 +378,7 @@ public class TalonFXSwerve extends SwerveMotor
   @Override
   public double getAppliedOutput()
   {
-    return motor.getDutyCycle().refresh().getValue();
+    return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
   }
 
   /**

swervelib/parser/Cache.java

@@ -0,0 +1,105 @@
+package swervelib.parser;
+
+import edu.wpi.first.wpilibj.RobotController;
+import java.util.function.Supplier;
+
+/**
+ * Cache for frequently requested data.
+ */
+public class Cache<T>
+{
+
+  /**
+   * Cached value.
+   */
+  private T           value;
+  /**
+   * Supplier for cached value.
+   */
+  private Supplier<T> supplier;
+  /**
+   * Timestamp in microseconds.
+   */
+  private long        timestamp;
+  /**
+   * Validity period in microseconds.
+   */
+  private long        validityPeriod;
+
+  /**
+   * Cache for arbitrary values.
+   *
+   * @param val            Value to cache.
+   * @param validityPeriod Validity period in milliseconds.
+   */
+  public Cache(Supplier<T> val, long validityPeriod)
+  {
+    supplier = val;
+    value = supplier.get();
+    timestamp = RobotController.getFPGATime();
+    this.validityPeriod = validityPeriod * 1000L;
+  }
+
+  /**
+   * Return whether the cache is stale.
+   *
+   * @return The stale state of the cache.
+   */
+  public boolean isStale()
+  {
+    return (RobotController.getFPGATime() - timestamp) > validityPeriod;
+  }
+
+  /**
+   * Update the cache value and timestamp.
+   *
+   * @return {@link Cache} used.
+   */
+  public Cache<T> update()
+  {
+    this.value = supplier.get();
+    this.timestamp = RobotController.getFPGATime();
+    return this;
+  }
+
+  /**
+   * Update the supplier to a new source. Updates the value and timestamp as well.
+   *
+   * @param supplier new supplier source.
+   * @return {@link Cache} for chaining.
+   */
+  public Cache<T> updateSupplier(Supplier<T> supplier)
+  {
+    this.supplier = supplier;
+    update();
+    return this;
+  }
+
+  /**
+   * Update the validity period for the cached value, also updates the value.
+   *
+   * @param validityPeriod The new validity period in milliseconds.
+   * @return {@link Cache} for chaining.
+   */
+  public Cache<T> updateValidityPeriod(long validityPeriod)
+  {
+    this.validityPeriod = validityPeriod * 1000L;
+    update();
+    return this;
+  }
+
+  /**
+   * Get the most up to date cached value.
+   *
+   * @return {@link T} updated to the latest cached version.
+   */
+  public T getValue()
+  {
+    if (isStale())
+    {
+      update();
+    }
+    return value;
+  }
+
+}