From bc72e8f831501e49e0ddfe323e660a7d201b8ca8 Mon Sep 17 00:00:00 2001
From: Bryce Roethel <br88c@br88c.dev>
Date: Sat, 17 Feb 2024 13:43:32 -0500
Subject: [PATCH] Sync with GRRBase

---
 .../java/org/team340/lib/GRRDashboard.java    |  11 +-
 .../org/team340/lib/HardwareSendables.java    |  29 ++--
 .../team340/lib/controller/Controller2.java   |  17 +--
 .../org/team340/lib/swerve/SwerveBase.java    |  45 ++++--
 .../org/team340/lib/swerve/SwerveModule.java  |   9 +-
 .../motors/vendors/SwerveSparkFlex.java       |   7 +-
 .../motors/vendors/SwerveSparkMax.java        |   6 +-
 .../lib/swerve/util/SwerveField2d.java        |   9 +-
 .../lib/swerve/util/SwerveSerializer.java     |  31 +---
 src/main/java/org/team340/lib/util/Sleep.java |  33 ++++
 .../config/rev/RelativeEncoderConfig.java     |  24 +--
 .../lib/util/config/rev/RevConfigBase.java    |  16 +-
 .../util/config/rev/RevConfigRegistry.java    | 141 ++++++++++++++++++
 .../lib/util/config/rev/RevConfigUtils.java   |  92 ------------
 ...g.java => SparkAbsoluteEncoderConfig.java} |  54 +++----
 .../lib/util/config/rev/SparkFlexConfig.java  |  56 +++----
 .../config/rev/SparkLimitSwitchConfig.java    |  20 ---
 .../lib/util/config/rev/SparkMaxConfig.java   |  56 +++----
 .../config/rev/SparkPIDControllerConfig.java  |  66 +++-----
 .../java/org/team340/robot/Constants.java     |  25 ++--
 src/main/java/org/team340/robot/Robot.java    |   2 +
 .../org/team340/robot/RobotContainer.java     |  18 +--
 vendordeps/Phoenix6.json                      |  48 +++---
 vendordeps/REVLib.json                        |  10 +-
 24 files changed, 394 insertions(+), 431 deletions(-)
 create mode 100644 src/main/java/org/team340/lib/util/Sleep.java
 create mode 100644 src/main/java/org/team340/lib/util/config/rev/RevConfigRegistry.java
 delete mode 100644 src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java
 rename src/main/java/org/team340/lib/util/config/rev/{AbsoluteEncoderConfig.java => SparkAbsoluteEncoderConfig.java} (71%)

diff --git a/src/main/java/org/team340/lib/GRRDashboard.java b/src/main/java/org/team340/lib/GRRDashboard.java
index a43ca23..bf6a13c 100644
--- a/src/main/java/org/team340/lib/GRRDashboard.java
+++ b/src/main/java/org/team340/lib/GRRDashboard.java
@@ -26,7 +26,6 @@
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Supplier;
 import org.team340.lib.controller.Controller2;
-import org.team340.lib.util.Math2;
 
 /**
  * Utility class for interfacing with the dashboard.
@@ -402,13 +401,13 @@ private static class RobotSendable implements Sendable {
         @Override
         public void initSendable(SendableBuilder builder) {
             builder.addBooleanProperty("blueAlliance", () -> DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue), null);
-            builder.addDoubleProperty("cpuTemperature", () -> Math2.toFixed(RobotController.getCPUTemp()), null);
+            builder.addDoubleProperty("cpuTemperature", RobotController::getCPUTemp, null);
             builder.addBooleanProperty("enabled", DriverStation::isEnabled, null);
-            builder.addIntegerProperty("matchTime", () -> (int) DriverStation.getMatchTime(), null);
-            builder.addDoubleProperty("ntUpdateTime", () -> Math2.toFixed(lastWatchdog, 1e-6), null);
-            builder.addDoubleProperty("powerUsage", () -> Math2.toFixed(lastPowerUsage), null);
+            builder.addDoubleProperty("matchTime", DriverStation::getMatchTime, null);
+            builder.addDoubleProperty("ntUpdateTime", () -> lastWatchdog, null);
+            builder.addDoubleProperty("powerUsage", () -> lastPowerUsage, null);
             builder.addIntegerProperty("timestamp", RobotController::getFPGATime, null);
-            builder.addDoubleProperty("voltage", () -> Math2.toFixed(RobotController.getBatteryVoltage()), null);
+            builder.addDoubleProperty("voltage", RobotController::getBatteryVoltage, null);
         }
     }
 }
diff --git a/src/main/java/org/team340/lib/HardwareSendables.java b/src/main/java/org/team340/lib/HardwareSendables.java
index d25be11..1ca79eb 100644
--- a/src/main/java/org/team340/lib/HardwareSendables.java
+++ b/src/main/java/org/team340/lib/HardwareSendables.java
@@ -12,7 +12,6 @@
 import java.util.List;
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Supplier;
-import org.team340.lib.util.Math2;
 
 // TODO Faults
 
@@ -172,13 +171,13 @@ public void initSendable(SendableBuilder builder) {
                         usageMutex.unlock();
                     }
 
-                    return Math2.toFixed(p);
+                    return p;
                 },
                 null
             );
-            builder.addDoubleProperty("powerUsage", () -> Math2.toFixed(usage), null);
-            builder.addDoubleProperty("voltage", () -> Math2.toFixed(voltage.get()), null);
-            builder.addDoubleProperty("current", () -> Math2.toFixed(current.get()), null);
+            builder.addDoubleProperty("powerUsage", () -> usage, null);
+            builder.addDoubleProperty("voltage", voltage::get, null);
+            builder.addDoubleProperty("current", current::get, null);
         }
     }
 
@@ -224,10 +223,10 @@ public Motor(
         @Override
         public void initSendable(SendableBuilder builder) {
             super.initSendable(builder);
-            builder.addDoubleProperty("output", () -> Math2.toFixed(output.get()), null);
-            builder.addDoubleProperty("temperature", () -> Math2.toFixed(temperature.get()), null);
-            builder.addDoubleProperty("velocity", () -> Math2.toFixed(velocity.get()), null);
-            builder.addDoubleProperty("position", () -> Math2.toFixed(position.get()), null);
+            builder.addDoubleProperty("output", output::get, null);
+            builder.addDoubleProperty("temperature", temperature::get, null);
+            builder.addDoubleProperty("velocity", velocity::get, null);
+            builder.addDoubleProperty("position", position::get, null);
         }
     }
 
@@ -255,8 +254,8 @@ public Encoder(String key, String label, Object api, Supplier<Double> velocity,
         @Override
         public void initSendable(SendableBuilder builder) {
             super.initSendable(builder);
-            builder.addDoubleProperty("velocity", () -> Math2.toFixed(velocity.get()), null);
-            builder.addDoubleProperty("position", () -> Math2.toFixed(position.get()), null);
+            builder.addDoubleProperty("velocity", velocity::get, null);
+            builder.addDoubleProperty("position", position::get, null);
         }
     }
 
@@ -287,9 +286,9 @@ public IMU(String key, String label, Object api, Supplier<Double> yaw, Supplier<
         @Override
         public void initSendable(SendableBuilder builder) {
             super.initSendable(builder);
-            builder.addDoubleProperty("yaw", () -> Math2.toFixed(yaw.get()), null);
-            builder.addDoubleProperty("pitch", () -> Math2.toFixed(pitch.get()), null);
-            builder.addDoubleProperty("roll", () -> Math2.toFixed(roll.get()), null);
+            builder.addDoubleProperty("yaw", yaw::get, null);
+            builder.addDoubleProperty("pitch", pitch::get, null);
+            builder.addDoubleProperty("roll", roll::get, null);
         }
     }
 
@@ -663,7 +662,7 @@ public PneumaticHub(String label, edu.wpi.first.wpilibj.PneumaticHub pneumaticHu
         @Override
         public void initSendable(SendableBuilder builder) {
             super.initSendable(builder);
-            builder.addDoubleProperty("pressure", () -> Math2.toFixed(pressure.get()), null);
+            builder.addDoubleProperty("pressure", pressure::get, null);
             builder.addBooleanProperty("pressureSwitchOn", pressureSwitchOn::get, null);
             builder.addBooleanProperty("compressorOn", compressorOn::get, null);
         }
diff --git a/src/main/java/org/team340/lib/controller/Controller2.java b/src/main/java/org/team340/lib/controller/Controller2.java
index b7f3d5b..aeb9023 100644
--- a/src/main/java/org/team340/lib/controller/Controller2.java
+++ b/src/main/java/org/team340/lib/controller/Controller2.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.team340.lib.GRRDashboard;
-import org.team340.lib.util.Math2;
 
 /**
  * A modified {@link CommandXboxController}.
@@ -453,19 +452,19 @@ public void initSendable(SendableBuilder builder) {
         builder.addIntegerProperty("pov", hid::getPOV, null);
 
         builder.addBooleanProperty("ls", hid::getLeftStickButton, null);
-        builder.addDoubleProperty("lx", () -> Math2.toFixed(getLeftX()), null);
-        builder.addDoubleProperty("ly", () -> Math2.toFixed(getLeftY()), null);
-        builder.addDoubleProperty("ln", () -> Math2.toFixed(Math.hypot(getLeftX(), getLeftY())), null);
-        builder.addDoubleProperty("lns", () -> Math2.toFixed(Math.hypot(super.getLeftX(), super.getLeftY())), null);
+        builder.addDoubleProperty("lx", this::getLeftX, null);
+        builder.addDoubleProperty("ly", this::getLeftY, null);
+        builder.addDoubleProperty("ln", () -> Math.hypot(getLeftX(), getLeftY()), null);
+        builder.addDoubleProperty("lns", () -> Math.hypot(super.getLeftX(), super.getLeftY()), null);
 
         builder.addBooleanProperty("rs", hid::getRightStickButton, null);
-        builder.addDoubleProperty("rx", () -> Math2.toFixed(getRightX()), null);
-        builder.addDoubleProperty("ry", () -> Math2.toFixed(getRightY()), null);
+        builder.addDoubleProperty("rx", this::getRightX, null);
+        builder.addDoubleProperty("ry", this::getRightY, null);
 
         builder.addBooleanProperty("lb", hid::getLeftBumper, null);
         builder.addBooleanProperty("rb", hid::getRightBumper, null);
 
-        builder.addDoubleProperty("lt", () -> Math2.toFixed(getLeftTriggerAxis()), null);
-        builder.addDoubleProperty("rt", () -> Math2.toFixed(getRightTriggerAxis()), null);
+        builder.addDoubleProperty("lt", this::getLeftTriggerAxis, null);
+        builder.addDoubleProperty("rt", this::getRightTriggerAxis, null);
     }
 }
diff --git a/src/main/java/org/team340/lib/swerve/SwerveBase.java b/src/main/java/org/team340/lib/swerve/SwerveBase.java
index e94cc87..5a72e43 100644
--- a/src/main/java/org/team340/lib/swerve/SwerveBase.java
+++ b/src/main/java/org/team340/lib/swerve/SwerveBase.java
@@ -180,21 +180,21 @@ public SwerveBase(String label, SwerveConfig config) {
     public void initSendable(SendableBuilder builder) {
         super.initSendable(builder);
 
-        builder.addDoubleProperty("velocityX", () -> Math2.toFixed(getVelocity(true).vxMetersPerSecond), null);
-        builder.addDoubleProperty("velocityY", () -> Math2.toFixed(getVelocity(true).vyMetersPerSecond), null);
-        builder.addDoubleProperty("velocityRot", () -> Math2.toFixed(getVelocity(true).omegaRadiansPerSecond), null);
-        builder.addDoubleProperty("velocityNorm", () -> SwerveSerializer.chassisSpeedsNorm(getVelocity(true), true), null);
+        builder.addDoubleProperty("velocityX", () -> getVelocity(true).vxMetersPerSecond, null);
+        builder.addDoubleProperty("velocityY", () -> getVelocity(true).vyMetersPerSecond, null);
+        builder.addDoubleProperty("velocityRot", () -> getVelocity(true).omegaRadiansPerSecond, null);
+        builder.addDoubleProperty("velocityNorm", () -> SwerveSerializer.chassisSpeedsNorm(getVelocity(true)), null);
 
-        builder.addDoubleProperty("odometryX", () -> Math2.toFixed(getPosition().getX()), null);
-        builder.addDoubleProperty("odometryY", () -> Math2.toFixed(getPosition().getY()), null);
-        builder.addDoubleProperty("odometryRot", () -> Math2.toFixed(getPosition().getRotation().getRadians()), null);
+        builder.addDoubleProperty("odometryX", () -> getPosition().getX(), null);
+        builder.addDoubleProperty("odometryY", () -> getPosition().getY(), null);
+        builder.addDoubleProperty("odometryRot", () -> getPosition().getRotation().getRadians(), null);
 
         builder.addDoubleArrayProperty(
             "desiredModuleStates",
-            () -> SwerveSerializer.moduleStatesDoubleArray(getDesiredModuleStates(), true),
+            () -> SwerveSerializer.moduleStatesDoubleArray(getDesiredModuleStates()),
             null
         );
-        builder.addDoubleArrayProperty("moduleStates", () -> SwerveSerializer.moduleStatesDoubleArray(getModuleStates(), true), null);
+        builder.addDoubleArrayProperty("moduleStates", () -> SwerveSerializer.moduleStatesDoubleArray(getModuleStates()), null);
 
         for (SwerveModule module : modules) {
             GRRDashboard.addSubsystemSendable(
@@ -202,9 +202,9 @@ public void initSendable(SendableBuilder builder) {
                 this,
                 SendableFactory.create(moduleBuilder -> {
                     moduleBuilder.publishConstString(".label", module.getLabel());
-                    moduleBuilder.addDoubleProperty("velocity", () -> Math2.toFixed(module.getVelocity()), null);
-                    moduleBuilder.addDoubleProperty("distance", () -> Math2.toFixed(module.getDistance()), null);
-                    moduleBuilder.addDoubleProperty("angle", () -> Math2.toFixed(module.getHeading()), null);
+                    moduleBuilder.addDoubleProperty("velocity", module::getVelocity, null);
+                    moduleBuilder.addDoubleProperty("distance", module::getDistance, null);
+                    moduleBuilder.addDoubleProperty("angle", module::getHeading, null);
                 })
             );
         }
@@ -336,23 +336,31 @@ protected void driveVelocity(double xV, double yV, double rotV, boolean fieldRel
      * Drives the robot using percents of its calculated max velocity while locked pointing at a position on the field.
      * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}.
      * @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}.
+     * @param angleOffset The offset to use when determining which side of the robot should face the point. (value in radians)
      * @param point The desired field relative position to point at (axis values in meters).
      * @param controller A profiled PID controller to use for translating to and maintaining the angle to the desired point.
      */
-    protected void driveAroundPoint(double x, double y, Translation2d point, ProfiledPIDController controller) {
-        driveAroundPointVelocity(x * config.getVelocity(), y * config.getVelocity(), point, controller);
+    protected void driveAroundPoint(double x, double y, double angleOffset, Translation2d point, ProfiledPIDController controller) {
+        driveAroundPointVelocity(x * config.getVelocity(), y * config.getVelocity(), angleOffset, point, controller);
     }
 
     /**
      * Drives the robot using velocity while locked pointing at a position on the field.
      * @param xV The desired {@code x} velocity in meters/second.
      * @param yV The desired {@code y} velocity in meters/second.
+     * @param angleOffset The offset to use when determining which side of the robot should face the point. (value in radians)
      * @param point The desired field relative position to point at (axis values in meters).
      * @param controller A profiled PID controller to use for translating to and maintaining the angle to the desired point.
      */
-    protected void driveAroundPointVelocity(double xV, double yV, Translation2d point, ProfiledPIDController controller) {
+    protected void driveAroundPointVelocity(
+        double xV,
+        double yV,
+        double angleOffset,
+        Translation2d point,
+        ProfiledPIDController controller
+    ) {
         Translation2d robotPoint = getPosition().getTranslation();
-        double angle = MathUtil.angleModulus(point.minus(robotPoint).getAngle().getRadians());
+        double angle = MathUtil.angleModulus(point.minus(robotPoint).getAngle().getRadians() + angleOffset);
         driveAngleVelocity(xV, yV, angle, controller);
     }
 
@@ -462,6 +470,11 @@ protected void driveVoltage(double voltage, Rotation2d heading) {
         for (int i = 0; i < modules.length; i++) {
             modules[i].setVoltage(voltage, heading);
         }
+        SwerveModuleState[] states = new SwerveModuleState[modules.length];
+        for (int i = 0; i < states.length; i++) {
+            states[i] = new SwerveModuleState(0, heading);
+        }
+        ratelimiter.setLastState(new SwerveRatelimiter.SwerveState(new ChassisSpeeds(), states));
     }
 
     /**
diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java
index 249b5fd..4af0311 100644
--- a/src/main/java/org/team340/lib/swerve/SwerveModule.java
+++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java
@@ -134,15 +134,14 @@ public SwerveModulePosition getModulePosition() {
     public void setDesiredState(SwerveModuleState state) {
         double moveSpeed = state.speedMetersPerSecond;
         double angleDiff = state.angle.rotateBy(Rotation2d.fromRadians(getHeading()).times(-1.0)).getRadians();
+        boolean flipped = false;
         if (Math.abs(angleDiff) > (Math2.HALF_PI)) {
             if (angleDiff > 0) angleDiff -= Math.PI; else angleDiff += Math.PI;
             moveSpeed *= -1.0;
+            flipped = true;
         }
 
-        double moveFF = moveFFController.calculate(
-            moveSpeed,
-            controlTimer.get() == 0 ? 0.0 : (moveSpeed - lastMoveSpeed) / controlTimer.get()
-        );
+        double moveFF = moveFFController.calculate(moveSpeed);
         double turnTarget = turnMotor.getPosition() + angleDiff;
 
         moveMotor.setReference(moveSpeed, moveFF);
@@ -157,7 +156,7 @@ public void setDesiredState(SwerveModuleState state) {
             simVelocity = moveSpeed;
         }
 
-        desiredState = state;
+        desiredState = flipped ? new SwerveModuleState(-state.speedMetersPerSecond, state.angle.rotateBy(Math2.ROTATION2D_PI)) : state;
         lastMoveSpeed = moveSpeed;
         controlTimer.restart();
     }
diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java
index 4a6bf8a..cb1d66e 100644
--- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java
+++ b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java
@@ -13,8 +13,8 @@
 import org.team340.lib.swerve.util.SwerveConversions;
 import org.team340.lib.util.Math2;
 import org.team340.lib.util.config.PIDConfig;
-import org.team340.lib.util.config.rev.AbsoluteEncoderConfig;
 import org.team340.lib.util.config.rev.RelativeEncoderConfig;
+import org.team340.lib.util.config.rev.SparkAbsoluteEncoderConfig;
 import org.team340.lib.util.config.rev.SparkFlexConfig;
 import org.team340.lib.util.config.rev.SparkFlexConfig.Frame;
 import org.team340.lib.util.config.rev.SparkPIDControllerConfig;
@@ -56,13 +56,12 @@ public SwerveSparkFlex(
 
         int periodMs = (int) (config.getPeriod() * 1000.0);
         int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0);
-        boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType());
+        boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()) && !isMoveMotor;
         double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian());
         PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID();
 
         new SparkFlexConfig()
             .clearFaults()
-            .restoreFactoryDefaults()
             .enableVoltageCompensation(config.getOptimalVoltage())
             .setSmartCurrentLimit((int) (isMoveMotor ? config.getMoveCurrentLimit() : config.getTurnCurrentLimit()))
             .setIdleMode(
@@ -91,7 +90,7 @@ public SwerveSparkFlex(
             .apply(sparkFlex, relativeEncoder);
 
         if (usingAttachedEncoder) {
-            new AbsoluteEncoderConfig()
+            new SparkAbsoluteEncoderConfig()
                 .setPositionConversionFactor(Math2.TWO_PI)
                 .setVelocityConversionFactor(Math2.TWO_PI / 60.0)
                 .setInverted(moduleConfig.getEncoderInverted())
diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java
index fa20b04..96afc49 100644
--- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java
+++ b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java
@@ -13,8 +13,8 @@
 import org.team340.lib.swerve.util.SwerveConversions;
 import org.team340.lib.util.Math2;
 import org.team340.lib.util.config.PIDConfig;
-import org.team340.lib.util.config.rev.AbsoluteEncoderConfig;
 import org.team340.lib.util.config.rev.RelativeEncoderConfig;
+import org.team340.lib.util.config.rev.SparkAbsoluteEncoderConfig;
 import org.team340.lib.util.config.rev.SparkMaxConfig;
 import org.team340.lib.util.config.rev.SparkMaxConfig.Frame;
 import org.team340.lib.util.config.rev.SparkPIDControllerConfig;
@@ -56,7 +56,7 @@ public SwerveSparkMax(
 
         int periodMs = (int) (config.getPeriod() * 1000.0);
         int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0);
-        boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType());
+        boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()) && !isMoveMotor;
         double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian());
         PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID();
 
@@ -91,7 +91,7 @@ public SwerveSparkMax(
             .apply(sparkMax, relativeEncoder);
 
         if (usingAttachedEncoder) {
-            new AbsoluteEncoderConfig()
+            new SparkAbsoluteEncoderConfig()
                 .setPositionConversionFactor(Math2.TWO_PI)
                 .setVelocityConversionFactor(Math2.TWO_PI / 60.0)
                 .setInverted(moduleConfig.getEncoderInverted())
diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java b/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java
index 4f35435..84771ae 100644
--- a/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java
+++ b/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java
@@ -1,7 +1,6 @@
 package org.team340.lib.swerve.util;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -38,12 +37,6 @@ public void update(Pose2d newPose) {
                 );
         }
 
-        setRobotPose(
-            new Pose2d(
-                Math2.toFixed(newPose.getX()),
-                Math2.toFixed(newPose.getY()),
-                Rotation2d.fromDegrees(Math2.toFixed(newPose.getRotation().getDegrees()))
-            )
-        );
+        setRobotPose(newPose);
     }
 }
diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java b/src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java
index 6e19ab4..275dabc 100644
--- a/src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java
+++ b/src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import org.team340.lib.util.Math2;
 
 /**
  * Serializers for swerve helper classes.
@@ -13,37 +12,13 @@ private SwerveSerializer() {
         throw new UnsupportedOperationException("This is a utility class!");
     }
 
-    /**
-     * Returns the norm of the provided chassis speeds in meters/second.
-     * @param speeds The chassis speeds.
-     */
-    public static double chassisSpeedsNorm(ChassisSpeeds speeds) {
-        return chassisSpeedsNorm(speeds, false);
-    }
-
     /**
      * Returns the norm of the provided chassis speeds in meters/second.
      * @param speeds The chassis speeds.
      * @param toFixed If the returned value should be rounded to a fixed precision.
      */
-    public static double chassisSpeedsNorm(ChassisSpeeds speeds, boolean toFixed) {
-        return Math2.toFixed(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond));
-    }
-
-    /**
-     * Converts module states into a single array. This is made to be compatible with AdvantageScope.
-     * Returns in the following format (heading is in radians):
-     * <pre>
-     * [
-     *   heading0, velocity0,
-     *   heading1, velocity1,
-     *   ...
-     * ]
-     * </pre>
-     * @param states The states to convert.
-     */
-    public static double[] moduleStatesDoubleArray(SwerveModuleState[] states) {
-        return moduleStatesDoubleArray(states, false);
+    public static double chassisSpeedsNorm(ChassisSpeeds speeds) {
+        return Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
     }
 
     /**
@@ -59,7 +34,7 @@ public static double[] moduleStatesDoubleArray(SwerveModuleState[] states) {
      * @param states The states to convert.
      * @param toFixed If the returned values should be rounded to a fixed precision.
      */
-    public static double[] moduleStatesDoubleArray(SwerveModuleState[] states, boolean toFixed) {
+    public static double[] moduleStatesDoubleArray(SwerveModuleState[] states) {
         double[] array = new double[states.length * 2];
         for (int i = 0; i < states.length; i++) {
             array[i * 2] = states[i].angle.getRadians();
diff --git a/src/main/java/org/team340/lib/util/Sleep.java b/src/main/java/org/team340/lib/util/Sleep.java
new file mode 100644
index 0000000..8c36cf7
--- /dev/null
+++ b/src/main/java/org/team340/lib/util/Sleep.java
@@ -0,0 +1,33 @@
+package org.team340.lib.util;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Utility class for sleeping the current thread, unless in simulation.
+ */
+public final class Sleep {
+
+    private Sleep() {
+        throw new UnsupportedOperationException("This is a utility class!");
+    }
+
+    /**
+     * Sleeps the thread for the specified duration in seconds.
+     * @param seconds The time to sleep for in seconds.
+     */
+    public static void seconds(double seconds) {
+        ms(seconds * 1000.0);
+    }
+
+    /**
+     * Sleeps the thread for the specified duration in milliseconds.
+     * @param ms The time to sleep for in milliseconds.
+     */
+    public static void ms(double ms) {
+        if (!RobotBase.isSimulation()) {
+            try {
+                Thread.sleep((long) ms);
+            } catch (Exception e) {}
+        }
+    }
+}
diff --git a/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java b/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java
index ab9091b..8ce0959 100644
--- a/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java
+++ b/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java
@@ -36,16 +36,6 @@ public RelativeEncoderConfig clone() {
      * @param relativeEncoder The relative encoder.
      */
     public void apply(CANSparkMax sparkMax, RelativeEncoder relativeEncoder) {
-        addStep(
-            re -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkMax.burnFlash();
-            },
-            re -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
         super.applySteps(relativeEncoder, "Spark Max (ID " + sparkMax.getDeviceId() + ") Relative Encoder");
     }
 
@@ -55,16 +45,6 @@ public void apply(CANSparkMax sparkMax, RelativeEncoder relativeEncoder) {
      * @param relativeEncoder The relative encoder.
      */
     public void apply(CANSparkFlex sparkFlex, RelativeEncoder relativeEncoder) {
-        addStep(
-            re -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkFlex.burnFlash();
-            },
-            re -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
         super.applySteps(relativeEncoder, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Relative Encoder");
     }
 
@@ -122,7 +102,7 @@ public RelativeEncoderConfig setMeasurementPeriod(int periodMs) {
     public RelativeEncoderConfig setPositionConversionFactor(double factor) {
         addStep(
             relativeEncoder -> relativeEncoder.setPositionConversionFactor(factor),
-            relativeEncoder -> Math2.epsilonEquals(relativeEncoder.getPositionConversionFactor(), factor, RevConfigUtils.EPSILON),
+            relativeEncoder -> Math2.epsilonEquals(relativeEncoder.getPositionConversionFactor(), factor, RevConfigRegistry.EPSILON),
             "Position Conversion Factor"
         );
         return this;
@@ -136,7 +116,7 @@ public RelativeEncoderConfig setPositionConversionFactor(double factor) {
     public RelativeEncoderConfig setVelocityConversionFactor(double factor) {
         addStep(
             relativeEncoder -> relativeEncoder.setVelocityConversionFactor(factor),
-            relativeEncoder -> Math2.epsilonEquals(relativeEncoder.getVelocityConversionFactor(), factor, RevConfigUtils.EPSILON),
+            relativeEncoder -> Math2.epsilonEquals(relativeEncoder.getVelocityConversionFactor(), factor, RevConfigRegistry.EPSILON),
             "Velocity Conversion Factor"
         );
         return this;
diff --git a/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java b/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java
index bb9cd9a..9b02be0 100644
--- a/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java
+++ b/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java
@@ -6,12 +6,16 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.Function;
+import org.team340.lib.util.Sleep;
 
 /**
  * Base class for configuring REV hardware.
  */
 abstract class RevConfigBase<T> {
 
+    private static final double CHECK_SLEEP = 25.0;
+    private static final int DEFAULT_SET_ITERATIONS = 3;
+
     private static final record RevConfigStep<T>(
         Function<T, REVLibError> applier,
         Function<T, Boolean> checker,
@@ -28,7 +32,7 @@ public RevConfigStep(Function<T, REVLibError> applier, Function<T, Boolean> chec
         }
 
         public RevConfigStep(Function<T, REVLibError> applier, Function<T, Boolean> checker, boolean trustCheck, String name) {
-            this(applier, checker, trustCheck, RevConfigUtils.DEFAULT_SET_ITERATIONS, name);
+            this(applier, checker, trustCheck, DEFAULT_SET_ITERATIONS, name);
         }
     }
 
@@ -84,7 +88,7 @@ void addStep(Function<T, REVLibError> applier, Function<T, Boolean> checker, boo
      */
     void applySteps(T hardware, String identifier) {
         for (RevConfigStep<T> step : configSteps) {
-            applyStep(hardware, identifier, step, new String[RevConfigUtils.DEFAULT_SET_ITERATIONS], RevConfigUtils.DEFAULT_SET_ITERATIONS);
+            applyStep(hardware, identifier, step, new String[DEFAULT_SET_ITERATIONS], DEFAULT_SET_ITERATIONS);
         }
     }
 
@@ -99,11 +103,7 @@ void applySteps(T hardware, String identifier) {
     private void applyStep(T hardware, String identifier, RevConfigStep<T> step, String[] results, int iterationsLeft) {
         try {
             REVLibError status = step.applier().apply(hardware);
-            if (!RobotBase.isSimulation()) {
-                try {
-                    Thread.sleep((long) RevConfigUtils.CHECK_SLEEP);
-                } catch (Exception e) {}
-            }
+            Sleep.ms(CHECK_SLEEP);
 
             boolean check = step.checker().apply(hardware);
             results[results.length - iterationsLeft] = status.name() + (!check ? " (Failed Check)" : "");
@@ -135,7 +135,7 @@ private void applyStep(T hardware, String identifier, RevConfigStep<T> step, Str
             }
 
             if (!hadSuccess) {
-                RevConfigUtils.addError(identifier + " \"" + step.name() + "\": " + resultsString);
+                RevConfigRegistry.addError(identifier + " \"" + step.name() + "\": " + resultsString);
             }
         } else {
             applyStep(hardware, identifier, step, results, iterationsLeft);
diff --git a/src/main/java/org/team340/lib/util/config/rev/RevConfigRegistry.java b/src/main/java/org/team340/lib/util/config/rev/RevConfigRegistry.java
new file mode 100644
index 0000000..1a83ef0
--- /dev/null
+++ b/src/main/java/org/team340/lib/util/config/rev/RevConfigRegistry.java
@@ -0,0 +1,141 @@
+package org.team340.lib.util.config.rev;
+
+import com.revrobotics.REVLibError;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.TimedRobot;
+import java.text.Collator;
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.Comparator;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.TreeSet;
+import java.util.function.Supplier;
+import org.team340.lib.util.Sleep;
+
+/**
+ * Utilities for REV hardware configs.
+ */
+public final class RevConfigRegistry {
+
+    public static final double EPSILON = 1e-4;
+
+    private static final double BURN_FLASH_START_SLEEP = 2000.0;
+    private static final double BURN_FLASH_INTERVAL = 10.0;
+    private static final double PERIODIC_INTERVAL = 500.0;
+
+    private static final List<Runnable> periodicCallbacks = new ArrayList<>();
+    private static final Map<String, Supplier<REVLibError>> burnFlashCallbacks = new LinkedHashMap<>();
+    private static final Collection<String> errors = new TreeSet<>(ErrorComparator.getInstance());
+
+    private RevConfigRegistry() {
+        throw new UnsupportedOperationException("This is a utility class!");
+    }
+
+    /**
+     * Saves a callback to a runnable for setting the periodic frame period.
+     * @param callback The runnable.
+     */
+    static void addPeriodic(Runnable callback) {
+        periodicCallbacks.add(callback);
+    }
+
+    /**
+     * Saves a callback to burn flash to a Spark motor controller.
+     * @param identifier The identifier of the Spark motor controller.
+     * @param callback A callback that burns flash and returns the result.
+     */
+    static void addBurnFlash(String identifier, Supplier<REVLibError> callback) {
+        burnFlashCallbacks.put(identifier, callback);
+    }
+
+    /**
+     * Saves a configuration error string to be logged.
+     * @param errorString The error string.
+     */
+    static void addError(String errorString) {
+        errors.add(errorString);
+    }
+
+    /**
+     * Initializes periodically applying frame periods from {@code periodicCallbacks} list.
+     * @param robot Robot to call {@link TimedRobot#addPeriodic(Runnable, double) addPeriodic()} on.
+     */
+    public static void init(TimedRobot robot) {
+        robot.addPeriodic(
+            () -> {
+                for (Runnable callback : periodicCallbacks) callback.run();
+            },
+            PERIODIC_INTERVAL
+        );
+    }
+
+    /**
+     * Burns flash to all registered Spark motor controllers.
+     */
+    public static void burnFlash() {
+        Sleep.ms(BURN_FLASH_START_SLEEP);
+
+        for (Map.Entry<String, Supplier<REVLibError>> entry : burnFlashCallbacks.entrySet()) {
+            String result;
+            try {
+                result = entry.getValue().get().name();
+                Sleep.ms(BURN_FLASH_INTERVAL);
+            } catch (Exception e) {
+                DriverStation.reportError(e.getMessage(), true);
+                result = e.getClass().getSimpleName();
+            }
+
+            if (!result.equals(REVLibError.kOk.name())) addError(entry.getKey() + " \"Burn Flash\": " + result);
+        }
+    }
+
+    /**
+     * Prints unsuccessful configurations to stdout.
+     * Useful for debugging, should be ran after initializing all hardware.
+     */
+    public static void printError() {
+        if (errors.size() <= 0) {
+            System.out.println("\nAll REV hardware configured successfully\n");
+        } else {
+            DriverStation.reportWarning("\nErrors while configuring " + errors.size() + " options on REV hardware:", false);
+
+            for (String errorString : errors) {
+                DriverStation.reportWarning("\t" + errorString, false);
+            }
+            DriverStation.reportWarning("\n", false);
+            errors.clear();
+        }
+    }
+
+    private static final class ErrorComparator implements Comparator<String> {
+
+        private static ErrorComparator instance;
+        private final Collator localeComparator = Collator.getInstance();
+
+        public static ErrorComparator getInstance() {
+            if (instance == null) instance = new ErrorComparator();
+            return instance;
+        }
+
+        private ErrorComparator() {}
+
+        @Override
+        public int compare(String arg0, String arg1) {
+            int idDiff;
+            try {
+                idDiff = Integer.parseInt(arg0.split(" ")[1]) - Integer.parseInt(arg1.split(" ")[1]);
+            } catch (Exception e) {
+                idDiff = 0;
+            }
+
+            if (idDiff == 0) {
+                int localeDiff = localeComparator.compare(arg0, arg1);
+                if (localeDiff == 0) return 1; else return localeDiff;
+            } else {
+                return idDiff;
+            }
+        }
+    }
+}
diff --git a/src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java b/src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java
deleted file mode 100644
index adf12c0..0000000
--- a/src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java
+++ /dev/null
@@ -1,92 +0,0 @@
-package org.team340.lib.util.config.rev;
-
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.RobotBase;
-import java.text.Collator;
-import java.util.Collection;
-import java.util.Comparator;
-import java.util.TreeSet;
-
-/**
- * Utilities for REV hardware configs.
- */
-public final class RevConfigUtils {
-
-    public static final double EPSILON = 1e-4;
-    public static final double BURN_FLASH_SLEEP = 100.0;
-    public static final double CHECK_SLEEP = 25.0;
-    public static final int DEFAULT_SET_ITERATIONS = 3;
-
-    private static final Collection<String> errors = new TreeSet<>(ErrorComparator.getInstance());
-
-    private RevConfigUtils() {
-        throw new UnsupportedOperationException("This is a utility class!");
-    }
-
-    /**
-     * Blocks the thread to ensure a safe burn to flash.
-     */
-    static void burnFlashSleep() {
-        if (!RobotBase.isSimulation()) {
-            try {
-                Thread.sleep((long) BURN_FLASH_SLEEP);
-            } catch (Exception e) {}
-        }
-    }
-
-    /**
-     * Saves a configuration error string to be logged.
-     * @param errorString The error string.
-     */
-    static void addError(String errorString) {
-        errors.add(errorString);
-    }
-
-    /**
-     * Prints unsuccessful configurations to stdout.
-     * Useful for debugging, should be ran after initializing all hardware.
-     */
-    public static void printError() {
-        if (errors.size() <= 0) {
-            System.out.println("\nAll REV hardware configured successfully\n");
-        } else {
-            DriverStation.reportWarning("\nErrors while configuring " + errors.size() + " options on REV hardware:", false);
-
-            for (String errorString : errors) {
-                DriverStation.reportWarning("\t" + errorString, false);
-            }
-            DriverStation.reportWarning("\n", false);
-            errors.clear();
-        }
-    }
-
-    private static final class ErrorComparator implements Comparator<String> {
-
-        private static ErrorComparator instance;
-        private final Collator localeComparator = Collator.getInstance();
-
-        public static ErrorComparator getInstance() {
-            if (instance == null) instance = new ErrorComparator();
-            return instance;
-        }
-
-        private ErrorComparator() {}
-
-        @Override
-        public int compare(String arg0, String arg1) {
-            int idDiff;
-            try {
-                idDiff = Integer.parseInt(arg0.split(" ")[1]) - Integer.parseInt(arg1.split(" ")[1]);
-            } catch (Exception e) {
-                idDiff = 0;
-            }
-
-            if (idDiff == 0) {
-                int localeDiff = localeComparator.compare(arg0, arg1);
-                if (localeDiff == 0) return 1; else return localeDiff;
-            } else {
-                return idDiff;
-            }
-        }
-    }
-}
diff --git a/src/main/java/org/team340/lib/util/config/rev/AbsoluteEncoderConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkAbsoluteEncoderConfig.java
similarity index 71%
rename from src/main/java/org/team340/lib/util/config/rev/AbsoluteEncoderConfig.java
rename to src/main/java/org/team340/lib/util/config/rev/SparkAbsoluteEncoderConfig.java
index a26faef..bf2d656 100644
--- a/src/main/java/org/team340/lib/util/config/rev/AbsoluteEncoderConfig.java
+++ b/src/main/java/org/team340/lib/util/config/rev/SparkAbsoluteEncoderConfig.java
@@ -1,33 +1,33 @@
 package org.team340.lib.util.config.rev;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkAbsoluteEncoder;
 import org.team340.lib.util.Math2;
 
 /**
- * Config builder for {@link AbsoluteEncoder}.
+ * Config builder for {@link SparkAbsoluteEncoder}.
  */
-public final class AbsoluteEncoderConfig extends RevConfigBase<AbsoluteEncoder> {
+public final class SparkAbsoluteEncoderConfig extends RevConfigBase<SparkAbsoluteEncoder> {
 
     /**
      * Creates an empty config.
      */
-    public AbsoluteEncoderConfig() {}
+    public SparkAbsoluteEncoderConfig() {}
 
     /**
      * Creates a config that copies the config steps from the base provided.
      * @param base The config to copy the steps from.
      */
-    private AbsoluteEncoderConfig(RevConfigBase<AbsoluteEncoder> base) {
+    private SparkAbsoluteEncoderConfig(RevConfigBase<SparkAbsoluteEncoder> base) {
         super(base);
     }
 
     /**
      * Clones this config.
      */
-    public AbsoluteEncoderConfig clone() {
-        return new AbsoluteEncoderConfig(this);
+    public SparkAbsoluteEncoderConfig clone() {
+        return new SparkAbsoluteEncoderConfig(this);
     }
 
     /**
@@ -35,17 +35,7 @@ public AbsoluteEncoderConfig clone() {
      * @param sparkMax The Spark Max the encoder is attached to.
      * @param absoluteEncoder The absolute encoder.
      */
-    public void apply(CANSparkMax sparkMax, AbsoluteEncoder absoluteEncoder) {
-        addStep(
-            ae -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkMax.burnFlash();
-            },
-            ae -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
+    public void apply(CANSparkMax sparkMax, SparkAbsoluteEncoder absoluteEncoder) {
         super.applySteps(absoluteEncoder, "Spark Max (ID " + sparkMax.getDeviceId() + ") Absolute Encoder");
     }
 
@@ -54,17 +44,7 @@ public void apply(CANSparkMax sparkMax, AbsoluteEncoder absoluteEncoder) {
      * @param sparkFlex The Spark Flex the encoder is attached to.
      * @param absoluteEncoder The absolute encoder.
      */
-    public void apply(CANSparkFlex sparkFlex, AbsoluteEncoder absoluteEncoder) {
-        addStep(
-            ae -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkFlex.burnFlash();
-            },
-            ae -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
+    public void apply(CANSparkFlex sparkFlex, SparkAbsoluteEncoder absoluteEncoder) {
         super.applySteps(absoluteEncoder, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Absolute Encoder");
     }
 
@@ -73,7 +53,7 @@ public void apply(CANSparkFlex sparkFlex, AbsoluteEncoder absoluteEncoder) {
      * either {@code 1}, {@code 2}, {@code 4}, {@code 8}, {@code 16}, {@code 32}, {@code 64}, or {@code 128}.
      * @param depth The average sampling depth of {@code 1}, {@code 2}, {@code 4}, {@code 8}, {@code 16}, {@code 32}, {@code 64}, or {@code 128}.
      */
-    public AbsoluteEncoderConfig setAverageDepth(int depth) {
+    public SparkAbsoluteEncoderConfig setAverageDepth(int depth) {
         addStep(
             absoluteEncoder -> absoluteEncoder.setAverageDepth(depth),
             absoluteEncoder -> absoluteEncoder.getAverageDepth() == depth,
@@ -86,7 +66,7 @@ public AbsoluteEncoderConfig setAverageDepth(int depth) {
      * Sets the phase of the absolute encoder so that it is set to be in phase with the motor itself.
      * @param inverted The phase of the encoder.
      */
-    public AbsoluteEncoderConfig setInverted(boolean inverted) {
+    public SparkAbsoluteEncoderConfig setInverted(boolean inverted) {
         addStep(
             absoluteEncoder -> absoluteEncoder.setInverted(inverted),
             absoluteEncoder -> absoluteEncoder.getInverted() == inverted,
@@ -100,10 +80,10 @@ public AbsoluteEncoderConfig setInverted(boolean inverted) {
      * by the native output units to give you position.
      * @param factor The conversion factor to multiply the native units (rotations) by.
      */
-    public AbsoluteEncoderConfig setPositionConversionFactor(double factor) {
+    public SparkAbsoluteEncoderConfig setPositionConversionFactor(double factor) {
         addStep(
             absoluteEncoder -> absoluteEncoder.setPositionConversionFactor(factor),
-            absoluteEncoder -> Math2.epsilonEquals(absoluteEncoder.getPositionConversionFactor(), factor, RevConfigUtils.EPSILON),
+            absoluteEncoder -> Math2.epsilonEquals(absoluteEncoder.getPositionConversionFactor(), factor, RevConfigRegistry.EPSILON),
             "Position Conversion Factor"
         );
         return this;
@@ -114,10 +94,10 @@ public AbsoluteEncoderConfig setPositionConversionFactor(double factor) {
      * by the native output units to give you velocity.
      * @param factor The conversion factor to multiply the native units (rotations per minute) by.
      */
-    public AbsoluteEncoderConfig setVelocityConversionFactor(double factor) {
+    public SparkAbsoluteEncoderConfig setVelocityConversionFactor(double factor) {
         addStep(
             absoluteEncoder -> absoluteEncoder.setVelocityConversionFactor(factor),
-            absoluteEncoder -> Math2.epsilonEquals(absoluteEncoder.getVelocityConversionFactor(), factor, RevConfigUtils.EPSILON),
+            absoluteEncoder -> Math2.epsilonEquals(absoluteEncoder.getVelocityConversionFactor(), factor, RevConfigRegistry.EPSILON),
             "Velocity Conversion Factor"
         );
         return this;
@@ -132,10 +112,10 @@ public AbsoluteEncoderConfig setVelocityConversionFactor(double factor) {
      * before calling this function.
      * @param offset The zero offset with the position conversion factor applied.
      */
-    public AbsoluteEncoderConfig setZeroOffset(double offset) {
+    public SparkAbsoluteEncoderConfig setZeroOffset(double offset) {
         addStep(
             absoluteEncoder -> absoluteEncoder.setZeroOffset(offset),
-            absoluteEncoder -> Math2.epsilonEquals(absoluteEncoder.getZeroOffset(), offset, RevConfigUtils.EPSILON),
+            absoluteEncoder -> Math2.epsilonEquals(absoluteEncoder.getZeroOffset(), offset, RevConfigRegistry.EPSILON),
             "Zero Offset"
         );
         return this;
diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java
index 048ac9a..126252e 100644
--- a/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java
+++ b/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java
@@ -3,8 +3,10 @@
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import com.revrobotics.REVLibError;
-import edu.wpi.first.wpilibj.RobotBase;
+import java.util.ArrayList;
+import java.util.List;
 import org.team340.lib.util.Math2;
+import org.team340.lib.util.Sleep;
 
 /**
  * Config builder for {@link CANSparkFlex}.
@@ -38,17 +40,9 @@ public SparkFlexConfig clone() {
      * @param sparkFlex The Spark Flex to apply the config to.
      */
     public void apply(CANSparkFlex sparkFlex) {
-        addStep(
-            sf -> {
-                RevConfigUtils.burnFlashSleep();
-                return sf.burnFlash();
-            },
-            sf -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
-        super.applySteps(sparkFlex, "Spark Flex (ID " + sparkFlex.getDeviceId() + ")");
+        String identifier = "Spark Flex (ID " + sparkFlex.getDeviceId() + ")";
+        super.applySteps(sparkFlex, identifier);
+        RevConfigRegistry.addBurnFlash(identifier, () -> sparkFlex.burnFlash());
     }
 
     /**
@@ -259,7 +253,7 @@ public SparkFlexConfig clearFaults() {
     public SparkFlexConfig disableVoltageCompensation() {
         addStep(
             sparkFlex -> sparkFlex.disableVoltageCompensation(),
-            sparkFlex -> Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), 0.0, RevConfigUtils.EPSILON),
+            sparkFlex -> Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON),
             "Disable Voltage Compensation"
         );
         return this;
@@ -286,7 +280,7 @@ public SparkFlexConfig enableSoftLimit(CANSparkFlex.SoftLimitDirection direction
     public SparkFlexConfig enableVoltageCompensation(double nominalVoltage) {
         addStep(
             sparkFlex -> sparkFlex.enableVoltageCompensation(nominalVoltage),
-            sparkFlex -> Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), nominalVoltage, RevConfigUtils.EPSILON),
+            sparkFlex -> Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), nominalVoltage, RevConfigRegistry.EPSILON),
             "Enable Voltage Compensation"
         );
         return this;
@@ -359,7 +353,7 @@ public SparkFlexConfig setCANTimeout(int milliseconds) {
     public SparkFlexConfig setClosedLoopRampRate(double rate) {
         addStep(
             sparkFlex -> sparkFlex.setClosedLoopRampRate(rate),
-            sparkFlex -> Math2.epsilonEquals(sparkFlex.getClosedLoopRampRate(), rate, RevConfigUtils.EPSILON),
+            sparkFlex -> Math2.epsilonEquals(sparkFlex.getClosedLoopRampRate(), rate, RevConfigRegistry.EPSILON),
             "Closed Loop Ramp Rate"
         );
         return this;
@@ -399,7 +393,7 @@ public SparkFlexConfig setInverted(boolean isInverted) {
     public SparkFlexConfig setOpenLoopRampRate(double rate) {
         addStep(
             sparkFlex -> sparkFlex.setOpenLoopRampRate(rate),
-            sparkFlex -> Math2.epsilonEquals(sparkFlex.getOpenLoopRampRate(), rate, RevConfigUtils.EPSILON),
+            sparkFlex -> Math2.epsilonEquals(sparkFlex.getOpenLoopRampRate(), rate, RevConfigRegistry.EPSILON),
             "Open Loop Ramp Rate"
         );
         return this;
@@ -413,7 +407,17 @@ public SparkFlexConfig setOpenLoopRampRate(double rate) {
      * @param periodMs The rate the controller sends the frame in milliseconds.
      */
     public SparkFlexConfig setPeriodicFramePeriod(Frame frame, int periodMs) {
-        addStep(sparkFlex -> sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs), "Periodic Frame Status " + frame.ordinal());
+        List<CANSparkFlex> applied = new ArrayList<>();
+        addStep(
+            sparkFlex -> {
+                if (!applied.contains(sparkFlex)) {
+                    RevConfigRegistry.addPeriodic(() -> sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs));
+                    applied.add(sparkFlex);
+                }
+                return sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs);
+            },
+            "Periodic Frame Status " + frame.ordinal()
+        );
         return this;
     }
 
@@ -517,7 +521,7 @@ public SparkFlexConfig setSmartCurrentLimit(int stallLimit, int freeLimit, int l
     public SparkFlexConfig setSoftLimit(CANSparkFlex.SoftLimitDirection direction, double limit) {
         addStep(
             sparkFlex -> sparkFlex.setSoftLimit(direction, (float) limit),
-            sparkFlex -> Math2.epsilonEquals(sparkFlex.getSoftLimit(direction), limit, RevConfigUtils.EPSILON),
+            sparkFlex -> Math2.epsilonEquals(sparkFlex.getSoftLimit(direction), limit, RevConfigRegistry.EPSILON),
             "Soft Limit"
         );
         return this;
@@ -531,13 +535,7 @@ public SparkFlexConfig restoreFactoryDefaults() {
         addStep(
             sparkFlex -> {
                 REVLibError res = sparkFlex.restoreFactoryDefaults();
-
-                if (!RobotBase.isSimulation()) {
-                    try {
-                        Thread.sleep((long) FACTORY_DEFAULTS_SLEEP);
-                    } catch (Exception e) {}
-                }
-
+                Sleep.ms(FACTORY_DEFAULTS_SLEEP);
                 return res;
             },
             "Restore Factory Defaults"
@@ -554,13 +552,7 @@ public SparkFlexConfig restoreFactoryDefaults(boolean persist) {
         addStep(
             sparkFlex -> {
                 REVLibError res = sparkFlex.restoreFactoryDefaults(persist);
-
-                if (!RobotBase.isSimulation()) {
-                    try {
-                        Thread.sleep((long) FACTORY_DEFAULTS_SLEEP);
-                    } catch (Exception e) {}
-                }
-
+                Sleep.ms(FACTORY_DEFAULTS_SLEEP);
                 return res;
             },
             "Restore Factory Defaults"
diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java
index cbd3d2b..68383ae 100644
--- a/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java
+++ b/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java
@@ -35,16 +35,6 @@ public SparkLimitSwitchConfig clone() {
      * @param limitSwitch The limit switch.
      */
     public void apply(CANSparkMax sparkMax, SparkLimitSwitch limitSwitch) {
-        addStep(
-            ls -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkMax.burnFlash();
-            },
-            ls -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
         super.applySteps(limitSwitch, "Spark Max (ID " + sparkMax.getDeviceId() + ") Limit Switch");
     }
 
@@ -54,16 +44,6 @@ public void apply(CANSparkMax sparkMax, SparkLimitSwitch limitSwitch) {
      * @param limitSwitch The limit switch.
      */
     public void apply(CANSparkFlex sparkFlex, SparkLimitSwitch limitSwitch) {
-        addStep(
-            ls -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkFlex.burnFlash();
-            },
-            ls -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
         super.applySteps(limitSwitch, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Limit Switch");
     }
 
diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java
index 9af895a..407fac8 100644
--- a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java
+++ b/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java
@@ -3,8 +3,10 @@
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.REVLibError;
-import edu.wpi.first.wpilibj.RobotBase;
+import java.util.ArrayList;
+import java.util.List;
 import org.team340.lib.util.Math2;
+import org.team340.lib.util.Sleep;
 
 /**
  * Config builder for {@link CANSparkMax}.
@@ -38,17 +40,9 @@ public SparkMaxConfig clone() {
      * @param sparkMax The Spark Max to apply the config to.
      */
     public void apply(CANSparkMax sparkMax) {
-        addStep(
-            sm -> {
-                RevConfigUtils.burnFlashSleep();
-                return sm.burnFlash();
-            },
-            sm -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
-        super.applySteps(sparkMax, "Spark Max (ID " + sparkMax.getDeviceId() + ")");
+        String identifier = "Spark Max (ID " + sparkMax.getDeviceId() + ")";
+        super.applySteps(sparkMax, identifier);
+        RevConfigRegistry.addBurnFlash(identifier, () -> sparkMax.burnFlash());
     }
 
     /**
@@ -259,7 +253,7 @@ public SparkMaxConfig clearFaults() {
     public SparkMaxConfig disableVoltageCompensation() {
         addStep(
             sparkMax -> sparkMax.disableVoltageCompensation(),
-            sparkMax -> Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), 0.0, RevConfigUtils.EPSILON),
+            sparkMax -> Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON),
             "Disable Voltage Compensation"
         );
         return this;
@@ -286,7 +280,7 @@ public SparkMaxConfig enableSoftLimit(CANSparkMax.SoftLimitDirection direction,
     public SparkMaxConfig enableVoltageCompensation(double nominalVoltage) {
         addStep(
             sparkMax -> sparkMax.enableVoltageCompensation(nominalVoltage),
-            sparkMax -> Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), nominalVoltage, RevConfigUtils.EPSILON),
+            sparkMax -> Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), nominalVoltage, RevConfigRegistry.EPSILON),
             "Enable Voltage Compensation"
         );
         return this;
@@ -359,7 +353,7 @@ public SparkMaxConfig setCANTimeout(int milliseconds) {
     public SparkMaxConfig setClosedLoopRampRate(double rate) {
         addStep(
             sparkMax -> sparkMax.setClosedLoopRampRate(rate),
-            sparkMax -> Math2.epsilonEquals(sparkMax.getClosedLoopRampRate(), rate, RevConfigUtils.EPSILON),
+            sparkMax -> Math2.epsilonEquals(sparkMax.getClosedLoopRampRate(), rate, RevConfigRegistry.EPSILON),
             "Closed Loop Ramp Rate"
         );
         return this;
@@ -399,7 +393,7 @@ public SparkMaxConfig setInverted(boolean isInverted) {
     public SparkMaxConfig setOpenLoopRampRate(double rate) {
         addStep(
             sparkMax -> sparkMax.setOpenLoopRampRate(rate),
-            sparkMax -> Math2.epsilonEquals(sparkMax.getOpenLoopRampRate(), rate, RevConfigUtils.EPSILON),
+            sparkMax -> Math2.epsilonEquals(sparkMax.getOpenLoopRampRate(), rate, RevConfigRegistry.EPSILON),
             "Open Loop Ramp Rate"
         );
         return this;
@@ -413,7 +407,17 @@ public SparkMaxConfig setOpenLoopRampRate(double rate) {
      * @param periodMs The rate the controller sends the frame in milliseconds.
      */
     public SparkMaxConfig setPeriodicFramePeriod(Frame frame, int periodMs) {
-        addStep(sparkMax -> sparkMax.setPeriodicFramePeriod(frame.frame, periodMs), "Periodic Frame Status " + frame.ordinal());
+        List<CANSparkMax> applied = new ArrayList<>();
+        addStep(
+            sparkMax -> {
+                if (!applied.contains(sparkMax)) {
+                    RevConfigRegistry.addPeriodic(() -> sparkMax.setPeriodicFramePeriod(frame.frame, periodMs));
+                    applied.add(sparkMax);
+                }
+                return sparkMax.setPeriodicFramePeriod(frame.frame, periodMs);
+            },
+            "Periodic Frame Status " + frame.ordinal()
+        );
         return this;
     }
 
@@ -517,7 +521,7 @@ public SparkMaxConfig setSmartCurrentLimit(int stallLimit, int freeLimit, int li
     public SparkMaxConfig setSoftLimit(CANSparkMax.SoftLimitDirection direction, double limit) {
         addStep(
             sparkMax -> sparkMax.setSoftLimit(direction, (float) limit),
-            sparkMax -> Math2.epsilonEquals(sparkMax.getSoftLimit(direction), limit, RevConfigUtils.EPSILON),
+            sparkMax -> Math2.epsilonEquals(sparkMax.getSoftLimit(direction), limit, RevConfigRegistry.EPSILON),
             "Soft Limit"
         );
         return this;
@@ -531,13 +535,7 @@ public SparkMaxConfig restoreFactoryDefaults() {
         addStep(
             sparkMax -> {
                 REVLibError res = sparkMax.restoreFactoryDefaults();
-
-                if (!RobotBase.isSimulation()) {
-                    try {
-                        Thread.sleep((long) FACTORY_DEFAULTS_SLEEP);
-                    } catch (Exception e) {}
-                }
-
+                Sleep.ms(FACTORY_DEFAULTS_SLEEP);
                 return res;
             },
             "Restore Factory Defaults"
@@ -554,13 +552,7 @@ public SparkMaxConfig restoreFactoryDefaults(boolean persist) {
         addStep(
             sparkMax -> {
                 REVLibError res = sparkMax.restoreFactoryDefaults(persist);
-
-                if (!RobotBase.isSimulation()) {
-                    try {
-                        Thread.sleep((long) FACTORY_DEFAULTS_SLEEP);
-                    } catch (Exception e) {}
-                }
-
+                Sleep.ms(FACTORY_DEFAULTS_SLEEP);
                 return res;
             },
             "Restore Factory Defaults"
diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java
index 21b0023..66b5f37 100644
--- a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java
+++ b/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java
@@ -40,16 +40,6 @@ public SparkPIDControllerConfig clone() {
      * @param pidController The PID controller.
      */
     public void apply(CANSparkMax sparkMax, SparkPIDController pidController) {
-        addStep(
-            pc -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkMax.burnFlash();
-            },
-            pc -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
         super.applySteps(pidController, "Spark Max (ID " + sparkMax.getDeviceId() + ") PID Controller");
     }
 
@@ -59,16 +49,6 @@ public void apply(CANSparkMax sparkMax, SparkPIDController pidController) {
      * @param pidController The PID controller.
      */
     public void apply(CANSparkFlex sparkFlex, SparkPIDController pidController) {
-        addStep(
-            pc -> {
-                RevConfigUtils.burnFlashSleep();
-                return sparkFlex.burnFlash();
-            },
-            pc -> true,
-            false,
-            1,
-            "Burn Flash"
-        );
         super.applySteps(pidController, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") PID Controller");
     }
 
@@ -79,7 +59,7 @@ public void apply(CANSparkFlex sparkFlex, SparkPIDController pidController) {
     public SparkPIDControllerConfig setD(double gain) {
         addStep(
             pidController -> pidController.setD(gain),
-            pidController -> Math2.epsilonEquals(pidController.getD(), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getD(), gain, RevConfigRegistry.EPSILON),
             "D Gain"
         );
         return this;
@@ -93,7 +73,7 @@ public SparkPIDControllerConfig setD(double gain) {
     public SparkPIDControllerConfig setD(double gain, int slotId) {
         addStep(
             pidController -> pidController.setD(gain, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getD(slotId), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getD(slotId), gain, RevConfigRegistry.EPSILON),
             "D Gain (Slot " + slotId + ")"
         );
         return this;
@@ -106,7 +86,7 @@ public SparkPIDControllerConfig setD(double gain, int slotId) {
     public SparkPIDControllerConfig setDFilter(double gain) {
         addStep(
             pidController -> pidController.setDFilter(gain),
-            pidController -> Math2.epsilonEquals(pidController.getDFilter(0), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getDFilter(0), gain, RevConfigRegistry.EPSILON),
             "D Filter"
         );
         return this;
@@ -120,7 +100,7 @@ public SparkPIDControllerConfig setDFilter(double gain) {
     public SparkPIDControllerConfig setDFilter(double gain, int slotId) {
         addStep(
             pidController -> pidController.setDFilter(gain, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getDFilter(slotId), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getDFilter(slotId), gain, RevConfigRegistry.EPSILON),
             "D Filter (Slot " + slotId + ")"
         );
         return this;
@@ -147,7 +127,7 @@ public SparkPIDControllerConfig setFeedbackDevice(MotorFeedbackSensor sensor) {
     public SparkPIDControllerConfig setFF(double gain) {
         addStep(
             pidController -> pidController.setFF(gain),
-            pidController -> Math2.epsilonEquals(pidController.getFF(), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getFF(), gain, RevConfigRegistry.EPSILON),
             "FF Gain"
         );
         return this;
@@ -161,7 +141,7 @@ public SparkPIDControllerConfig setFF(double gain) {
     public SparkPIDControllerConfig setFF(double gain, int slotId) {
         addStep(
             pidController -> pidController.setFF(gain, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getFF(slotId), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getFF(slotId), gain, RevConfigRegistry.EPSILON),
             "FF Gain (Slot " + slotId + ")"
         );
         return this;
@@ -174,7 +154,7 @@ public SparkPIDControllerConfig setFF(double gain, int slotId) {
     public SparkPIDControllerConfig setI(double gain) {
         addStep(
             pidController -> pidController.setI(gain),
-            pidController -> Math2.epsilonEquals(pidController.getI(), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getI(), gain, RevConfigRegistry.EPSILON),
             "I Gain"
         );
         return this;
@@ -188,7 +168,7 @@ public SparkPIDControllerConfig setI(double gain) {
     public SparkPIDControllerConfig setI(double gain, int slotId) {
         addStep(
             pidController -> pidController.setI(gain, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getI(slotId), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getI(slotId), gain, RevConfigRegistry.EPSILON),
             "I Gain (Slot " + slotId + ")"
         );
         return this;
@@ -203,7 +183,7 @@ public SparkPIDControllerConfig setI(double gain, int slotId) {
     public SparkPIDControllerConfig setIMaxAccum(double iMaxAccum, int slotId) {
         addStep(
             pidController -> pidController.setIMaxAccum(iMaxAccum, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getIMaxAccum(slotId), iMaxAccum, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getIMaxAccum(slotId), iMaxAccum, RevConfigRegistry.EPSILON),
             "I Max Accumulator (Slot " + slotId + ")"
         );
         return this;
@@ -217,7 +197,7 @@ public SparkPIDControllerConfig setIMaxAccum(double iMaxAccum, int slotId) {
     public SparkPIDControllerConfig setIZone(double iZone) {
         addStep(
             pidController -> pidController.setIZone(iZone),
-            pidController -> Math2.epsilonEquals(pidController.getIZone(), iZone, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getIZone(), iZone, RevConfigRegistry.EPSILON),
             "I Zone"
         );
         return this;
@@ -232,7 +212,7 @@ public SparkPIDControllerConfig setIZone(double iZone) {
     public SparkPIDControllerConfig setIZone(double iZone, int slotId) {
         addStep(
             pidController -> pidController.setIZone(iZone, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getIZone(slotId), iZone, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getIZone(slotId), iZone, RevConfigRegistry.EPSILON),
             "I Zone (Slot " + slotId + ")"
         );
         return this;
@@ -247,8 +227,8 @@ public SparkPIDControllerConfig setOutputRange(double min, double max) {
         addStep(
             pidController -> pidController.setOutputRange(min, max),
             pidController ->
-                Math2.epsilonEquals(pidController.getOutputMin(), min, RevConfigUtils.EPSILON) &&
-                Math2.epsilonEquals(pidController.getOutputMax(), max, RevConfigUtils.EPSILON),
+                Math2.epsilonEquals(pidController.getOutputMin(), min, RevConfigRegistry.EPSILON) &&
+                Math2.epsilonEquals(pidController.getOutputMax(), max, RevConfigRegistry.EPSILON),
             "Output Range"
         );
         return this;
@@ -264,8 +244,8 @@ public SparkPIDControllerConfig setOutputRange(double min, double max, int slotI
         addStep(
             pidController -> pidController.setOutputRange(min, max, slotId),
             pidController ->
-                Math2.epsilonEquals(pidController.getOutputMin(slotId), min, RevConfigUtils.EPSILON) &&
-                Math2.epsilonEquals(pidController.getOutputMax(slotId), max, RevConfigUtils.EPSILON),
+                Math2.epsilonEquals(pidController.getOutputMin(slotId), min, RevConfigRegistry.EPSILON) &&
+                Math2.epsilonEquals(pidController.getOutputMax(slotId), max, RevConfigRegistry.EPSILON),
             "Output Range (Slot " + slotId + ")"
         );
         return this;
@@ -278,7 +258,7 @@ public SparkPIDControllerConfig setOutputRange(double min, double max, int slotI
     public SparkPIDControllerConfig setP(double gain) {
         addStep(
             pidController -> pidController.setP(gain),
-            pidController -> Math2.epsilonEquals(pidController.getP(), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getP(), gain, RevConfigRegistry.EPSILON),
             "P Gain"
         );
         return this;
@@ -292,7 +272,7 @@ public SparkPIDControllerConfig setP(double gain) {
     public SparkPIDControllerConfig setP(double gain, int slotId) {
         addStep(
             pidController -> pidController.setP(gain, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getP(slotId), gain, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getP(slotId), gain, RevConfigRegistry.EPSILON),
             "P Gain (Slot " + slotId + ")"
         );
         return this;
@@ -318,7 +298,7 @@ public SparkPIDControllerConfig setPositionPIDWrappingEnabled(boolean enable) {
     public SparkPIDControllerConfig setPositionPIDWrappingMaxInput(double max) {
         addStep(
             pidController -> pidController.setPositionPIDWrappingMaxInput(max),
-            pidController -> Math2.epsilonEquals(pidController.getPositionPIDWrappingMaxInput(), max, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getPositionPIDWrappingMaxInput(), max, RevConfigRegistry.EPSILON),
             "Position PID Wrapping Max Input"
         );
         return this;
@@ -331,7 +311,7 @@ public SparkPIDControllerConfig setPositionPIDWrappingMaxInput(double max) {
     public SparkPIDControllerConfig setPositionPIDWrappingMinInput(double min) {
         addStep(
             pidController -> pidController.setPositionPIDWrappingMinInput(min),
-            pidController -> Math2.epsilonEquals(pidController.getPositionPIDWrappingMinInput(), min, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getPositionPIDWrappingMinInput(), min, RevConfigRegistry.EPSILON),
             "Position PID Wrapping Min Input"
         );
         return this;
@@ -472,7 +452,7 @@ public SparkPIDControllerConfig setSmartMotionAllowedClosedLoopError(double allo
         addStep(
             pidController -> pidController.setSmartMotionAllowedClosedLoopError(allowedErr, slotId),
             pidController ->
-                Math2.epsilonEquals(pidController.getSmartMotionAllowedClosedLoopError(slotId), allowedErr, RevConfigUtils.EPSILON),
+                Math2.epsilonEquals(pidController.getSmartMotionAllowedClosedLoopError(slotId), allowedErr, RevConfigRegistry.EPSILON),
             "Smart Motion Allowed Closed Loop Error (Slot " + slotId + ")"
         );
         return this;
@@ -487,7 +467,7 @@ public SparkPIDControllerConfig setSmartMotionAllowedClosedLoopError(double allo
     public SparkPIDControllerConfig setSmartMotionMaxAccel(double maxAccel, int slotId) {
         addStep(
             pidController -> pidController.setSmartMotionMaxAccel(maxAccel, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxAccel(slotId), maxAccel, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxAccel(slotId), maxAccel, RevConfigRegistry.EPSILON),
             "Smart Motion Max Acceleration (Slot " + slotId + ")"
         );
         return this;
@@ -502,7 +482,7 @@ public SparkPIDControllerConfig setSmartMotionMaxAccel(double maxAccel, int slot
     public SparkPIDControllerConfig setSmartMotionMaxVelocity(double maxVel, int slotId) {
         addStep(
             pidController -> pidController.setSmartMotionMaxVelocity(maxVel, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxVelocity(slotId), maxVel, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxVelocity(slotId), maxVel, RevConfigRegistry.EPSILON),
             "Smart Motion Max Velocity (Slot " + slotId + ")"
         );
         return this;
@@ -516,7 +496,7 @@ public SparkPIDControllerConfig setSmartMotionMaxVelocity(double maxVel, int slo
     public SparkPIDControllerConfig setSmartMotionMinOutputVelocity(double minVel, int slotId) {
         addStep(
             pidController -> pidController.setSmartMotionMinOutputVelocity(minVel, slotId),
-            pidController -> Math2.epsilonEquals(pidController.getSmartMotionMinOutputVelocity(slotId), minVel, RevConfigUtils.EPSILON),
+            pidController -> Math2.epsilonEquals(pidController.getSmartMotionMinOutputVelocity(slotId), minVel, RevConfigRegistry.EPSILON),
             "Smart Motion Min Velocity (Slot " + slotId + ")"
         );
         return this;
diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java
index 426d60b..e237170 100644
--- a/src/main/java/org/team340/robot/Constants.java
+++ b/src/main/java/org/team340/robot/Constants.java
@@ -1,5 +1,8 @@
 package org.team340.robot;
 
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime;
 import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
@@ -33,7 +36,7 @@ public static final class ControllerConstants {
         public static final Controller2Config DRIVER = new Controller2Config()
             .setLabel("Driver")
             .setPort(0)
-            .setJoystickDeadband(0.1)
+            .setJoystickDeadband(0.15)
             .setJoystickThreshold(0.7)
             .setTriggerDeadband(0.1)
             .setTriggerThreshold(0.1)
@@ -51,7 +54,7 @@ public static final class ControllerConstants {
             .setRightProfile("joystickprofiles/CoDriverRight.json");
 
         public static final double DRIVE_EXP = 1.0;
-        public static final double DRIVE_MULTIPLIER = 0.75;
+        public static final double DRIVE_MULTIPLIER = 0.95;
         public static final double DRIVE_MULTIPLIER_MODIFIED = 0.95;
 
         public static final double DRIVE_ROT_EXP = 2.0;
@@ -109,19 +112,19 @@ public static final class SwerveConstants {
         public static final SwerveConfig CONFIG = new SwerveConfig()
             .useADIS16470(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, Port.kOnboardCS0, CalibrationTime._4s)
             .setPeriod(PERIOD)
-            .setMovePID(0.001, 0.0, 0.0, 0.0)
-            .setMoveFF(0.0, 2.84, 0.0)
-            .setTurnPID(0.75, 0.0, 16.0, 0.0)
+            .setMovePID(0.0125, 0.001, 0.005, 0.075)
+            .setMoveFF(0.12229, 2.84820, 0.46966)
+            .setTurnPID(0.65, 0.001, 3.0, 0.01)
             .setRampRate(0.03, 0.03)
             .setMotorTypes(SwerveMotor.Type.SPARK_MAX_BRUSHLESS, SwerveMotor.Type.SPARK_MAX_BRUSHLESS)
-            .setMaxSpeeds(4.05, 7.12)
-            .setRatelimits(8.23, 30.35)
-            .setPowerProperties(VOLTAGE, 60.0, 30.0)
+            .setMaxSpeeds(4.2, 9.7)
+            .setRatelimits(10.2, 32.25)
+            .setPowerProperties(VOLTAGE, 100.0, 40.0)
             .setMechanicalProperties(7.13, 13.71, 4.0)
             .setDiscretizationLookahead(0.020)
-            .setOdometryPeriod(0.005)
+            .setOdometryPeriod(0.008)
             .setOdometryStd(0.1, 0.1, 0.1)
-            .setSysIdConfig(new SysIdRoutine.Config())
+            .setSysIdConfig(new SysIdRoutine.Config(Volts.of(1.0).per(Seconds.of(0.4)), Volts.of(7.0), Seconds.of(5.5)))
             .setFieldSize(FIELD_LENGTH, FIELD_WIDTH)
             .addModule(FRONT_LEFT)
             .addModule(BACK_LEFT)
@@ -129,6 +132,6 @@ public static final class SwerveConstants {
             .addModule(FRONT_RIGHT);
 
         public static final PIDConfig ROT_PID = new PIDConfig(7.0, 0.0, 0.5, 0.0);
-        public static final Constraints ROT_CONSTRAINTS = new Constraints(6.0, 12.5);
+        public static final Constraints ROT_CONSTRAINTS = new Constraints(9.5, 28.5);
     }
 }
diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java
index c82d193..811871a 100644
--- a/src/main/java/org/team340/robot/Robot.java
+++ b/src/main/java/org/team340/robot/Robot.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import org.team340.lib.GRRDashboard;
+import org.team340.lib.util.config.rev.RevConfigRegistry;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -28,6 +29,7 @@ public void robotInit() {
 
         DataLogManager.logNetworkTables(true);
         GRRDashboard.initAsync(this, Constants.TELEMETRY_PERIOD, Constants.POWER_USAGE_PERIOD);
+        RevConfigRegistry.init(this);
         RobotContainer.init();
     }
 
diff --git a/src/main/java/org/team340/robot/RobotContainer.java b/src/main/java/org/team340/robot/RobotContainer.java
index 2d1e7d3..59df606 100644
--- a/src/main/java/org/team340/robot/RobotContainer.java
+++ b/src/main/java/org/team340/robot/RobotContainer.java
@@ -2,11 +2,11 @@
 
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import org.team340.lib.GRRDashboard;
 import org.team340.lib.controller.Controller2;
 import org.team340.lib.util.Math2;
-import org.team340.lib.util.config.rev.RevConfigUtils;
+import org.team340.lib.util.config.rev.RevConfigRegistry;
 import org.team340.robot.Constants.ControllerConstants;
 import org.team340.robot.commands.SystemsCheck;
 import org.team340.robot.subsystems.Swerve;
@@ -47,7 +47,7 @@ public static void init() {
         GRRDashboard.setSystemsCheck(SystemsCheck.command());
 
         // Print errors from REV hardware initialization.
-        RevConfigUtils.printError();
+        RevConfigRegistry.printError();
 
         // Configure bindings and autos.
         configBindings();
@@ -75,14 +75,10 @@ private static void configBindings() {
         // Right Bumper => Lock wheels
         driver.rightBumper().whileTrue(swerve.lock());
 
-        // A => SysId quasistatic forward
-        driver.a().whileTrue(swerve.sysIdQuasistatic(Direction.kForward));
-        // B => SysId dynamic forward
-        driver.b().whileTrue(swerve.sysIdDynamic(Direction.kForward));
-        // X => SysId dynamic reverse
-        driver.x().whileTrue(swerve.sysIdDynamic(Direction.kReverse));
-        // Y => SysId quasistatic reverse
-        driver.y().whileTrue(swerve.sysIdQuasistatic(Direction.kReverse));
+        driver.a().whileTrue(swerve.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+        driver.b().whileTrue(swerve.sysIdDynamic(SysIdRoutine.Direction.kForward));
+        driver.x().whileTrue(swerve.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+        driver.y().whileTrue(swerve.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
         /**
          * Co-driver bindings.
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json
index 69a4079..2b7d172 100644
--- a/vendordeps/Phoenix6.json
+++ b/vendordeps/Phoenix6.json
@@ -1,7 +1,7 @@
 {
     "fileName": "Phoenix6.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "24.1.0",
+    "version": "24.2.0",
     "frcYear": 2024,
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "24.1.0"
+            "version": "24.2.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -39,7 +39,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -52,7 +52,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -65,7 +65,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonFX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -78,7 +78,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -91,7 +91,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -104,7 +104,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -117,7 +117,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -130,7 +130,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -143,7 +143,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -158,7 +158,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -173,7 +173,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -188,7 +188,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -203,7 +203,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -218,7 +218,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -233,7 +233,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonFX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -248,7 +248,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -263,7 +263,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -278,7 +278,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -293,7 +293,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +308,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -323,7 +323,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "24.1.0",
+            "version": "24.2.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index 0f3520e..a829581 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2024.2.0",
+    "version": "2024.2.1",
     "frcYear": "2024",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2024.2.0"
+            "version": "2024.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.0",
+            "version": "2024.2.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -37,7 +37,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2024.2.0",
+            "version": "2024.2.1",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -55,7 +55,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.0",
+            "version": "2024.2.1",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,