assorted pre-2026 cleanup, tweaks

src/main/java/frc/robot/RobotContainer.java

@@ -111,7 +111,7 @@ private SwerveDriveIO configureDrive() {
     return switch (Constants.CURRENT_MODE) {
       case REAL ->
           // Real robot, instantiate hardware IO implementations
-          new SwerveDriveSubsystem(
+          new SwerveDriveReal(
               new GyroIOPigeon2(),
               new ModuleIOTalonFX(TunerConstants.FrontLeft),
               new ModuleIOTalonFX(TunerConstants.FrontRight),
@@ -122,7 +122,7 @@ private SwerveDriveIO configureDrive() {
           new SwerveDriveSim();
       default ->
           // Replayed robot, disable IO implementations
-          new SwerveDriveSubsystem(
+          new SwerveDriveReal(
               new GyroIO() {},
               new ModuleIO() {},
               new ModuleIO() {},

src/main/java/frc/robot/commands/DriveCommands.java

@@ -329,21 +329,16 @@ public static Command goToTransformWithPathFinder(
    * using the provided control points.
    *
    * @param drive The drive subsystem
-   * @param startSupplier Pose to start at
-   * @param endSupplier Pose to end at
+   * @param start Pose to start at
+   * @param end Pose to end at
    * @param controls Control points
    */
   public static Command followCurve(
-      SwerveDriveIO drive,
-      Supplier<Pose2d> startSupplier,
-      Supplier<Pose2d> endSupplier,
-      Supplier<Pose2d[]> controls) {
+      SwerveDriveIO drive, Pose2d start, Pose2d end, Pose2d... controls) {
 
-    Pose2d start = startSupplier.get();
-    Pose2d end = endSupplier.get();
     List<Translation2d> pts = new ArrayList<>();
     pts.add(start.getTranslation());
-    for (Pose2d pose : controls.get()) {
+    for (Pose2d pose : controls) {
       pts.add(pose.getTranslation());
     }
     pts.add(end.getTranslation());
@@ -395,18 +390,20 @@ public static Command followCurve(
       sampled[i] = new Pose2d(pos, rot);
     }
 
-    return followPoses(drive, () -> sampled);
+    return followPoses(drive, sampled);
   }
 
   /**
    * Navigates to the first {@code Pose2d} provided, then begins following a path constructed from
    * the provided poses.
    *
    * @param drive The drive subsystem
-   * @param pointArraySupplier {@code Pose2d} points to construct a path out of
+   * @param points {@code Pose2d} points to construct a path out of
    */
-  public static Command followPoses(SwerveDriveIO drive, Supplier<Pose2d[]> pointArraySupplier) {
-    Pose2d[] points = pointArraySupplier.get();
+  public static Command followPoses(SwerveDriveIO drive, Pose2d... points) {
+    if (points == null || points.length == 0) {
+      return new InstantCommand(() -> {});
+    }
     List<Waypoint> waypoints = PathPlannerPath.waypointsFromPoses(points);
     PathPlannerPath path =
         new PathPlannerPath(
@@ -431,11 +428,13 @@ public static Command followPoses(SwerveDriveIO drive, Supplier<Pose2d[]> pointA
    * @param drive The drive subsystem
    * @param transitionVelocity The speed in m/s that should be maintained from the initial pathing
    *     when starting to follow the actual path
-   * @param pointArraySupplier {@code Pose2d} points to construct a path out of
+   * @param points {@code Pose2d} points to construct a path out of
    */
   public static Command followPoses(
-      SwerveDriveIO drive, double transitionVelocity, Supplier<Pose2d[]> pointArraySupplier) {
-    Pose2d[] points = pointArraySupplier.get();
+      SwerveDriveIO drive, double transitionVelocity, Pose2d... points) {
+    if (points == null || points.length == 0) {
+      return new InstantCommand(() -> {});
+    }
     List<Waypoint> waypoints = PathPlannerPath.waypointsFromPoses(points);
     PathPlannerPath path =
         new PathPlannerPath(

src/main/java/frc/robot/generated/TunerConstants.java

@@ -20,6 +20,8 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.*;
 
+// TODO: Slip Current, Steer/Drive Current Limits, Steer Gains
+
 // Generated using https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
 public class TunerConstants {
   // Both sets of gains need to be tuned to your individual robot.
@@ -73,7 +75,14 @@ public class TunerConstants {
   private static final Current kSlipCurrent = Amps.of(100.0);
   // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
   // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
-  private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+  private static final TalonFXConfiguration driveInitialConfigs =
+      new TalonFXConfiguration()
+          .withCurrentLimits(
+              new CurrentLimitsConfigs()
+                  .withSupplyCurrentLimit(40)
+                  .withSupplyCurrentLimitEnable(true)
+                  .withStatorCurrentLimit(40)
+                  .withStatorCurrentLimitEnable(true));
   private static final TalonFXConfiguration steerInitialConfigs =
       new TalonFXConfiguration()
           .withCurrentLimits(
@@ -82,7 +91,9 @@ public class TunerConstants {
                   // low
                   // stator current limit to help avoid brownouts without impacting performance.
                   .withStatorCurrentLimit(Amps.of(60))
-                  .withStatorCurrentLimitEnable(true));
+                  .withStatorCurrentLimitEnable(true)
+                  .withSupplyCurrentLimit(Amps.of(40))
+                  .withSupplyCurrentLimitEnable(true));
   private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
   // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
   private static final Pigeon2Configuration pigeonConfigs = null;

src/main/java/frc/robot/subsystems/drive/GyroIONavX.java

@@ -8,8 +8,7 @@
 
 /** IO implementation for NavX. */
 public class GyroIONavX implements GyroIO {
-  private final AHRS navX =
-      new AHRS(NavXComType.kMXP_SPI, (byte) SwerveDriveSubsystem.ODOMETRY_FREQUENCY);
+  private final AHRS navX = new AHRS(NavXComType.kMXP_SPI, (byte) SwerveDriveIO.ODOMETRY_FREQUENCY);
   private final Queue<Double> yawPositionQueue;
   private final Queue<Double> yawTimestampQueue;
 

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -26,7 +26,7 @@ public class GyroIOPigeon2 implements GyroIO {
   public GyroIOPigeon2() {
     pigeon.getConfigurator().apply(new Pigeon2Configuration());
     pigeon.getConfigurator().setYaw(0.0);
-    yaw.setUpdateFrequency(SwerveDriveSubsystem.ODOMETRY_FREQUENCY);
+    yaw.setUpdateFrequency(SwerveDriveIO.ODOMETRY_FREQUENCY);
     yawVelocity.setUpdateFrequency(50.0);
     pigeon.optimizeBusUtilization();
     yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -157,7 +157,7 @@ public ModuleIOTalonFX(
 
     // Configure periodic frames
     BaseStatusSignal.setUpdateFrequencyForAll(
-        SwerveDriveSubsystem.ODOMETRY_FREQUENCY, drivePosition, turnPosition);
+        SwerveDriveIO.ODOMETRY_FREQUENCY, drivePosition, turnPosition);
     BaseStatusSignal.setUpdateFrequencyForAll(
         50.0,
         driveVelocity,

src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java

@@ -58,7 +58,7 @@ public synchronized void start() {
   public Queue<Double> registerSignal(StatusSignal<Angle> signal) {
     Queue<Double> queue = new ArrayBlockingQueue<>(20);
     signalsLock.lock();
-    SwerveDriveSubsystem.ODOMETRY_LOCK.lock();
+    SwerveDriveIO.ODOMETRY_LOCK.lock();
     try {
       BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1];
       System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length);
@@ -67,7 +67,7 @@ public Queue<Double> registerSignal(StatusSignal<Angle> signal) {
       phoenixQueues.add(queue);
     } finally {
       signalsLock.unlock();
-      SwerveDriveSubsystem.ODOMETRY_LOCK.unlock();
+      SwerveDriveIO.ODOMETRY_LOCK.unlock();
     }
     return queue;
   }
@@ -76,25 +76,25 @@ public Queue<Double> registerSignal(StatusSignal<Angle> signal) {
   public Queue<Double> registerSignal(DoubleSupplier signal) {
     Queue<Double> queue = new ArrayBlockingQueue<>(20);
     signalsLock.lock();
-    SwerveDriveSubsystem.ODOMETRY_LOCK.lock();
+    SwerveDriveIO.ODOMETRY_LOCK.lock();
     try {
       genericSignals.add(signal);
       genericQueues.add(queue);
     } finally {
       signalsLock.unlock();
-      SwerveDriveSubsystem.ODOMETRY_LOCK.unlock();
+      SwerveDriveIO.ODOMETRY_LOCK.unlock();
     }
     return queue;
   }
 
   /** Returns a new queue that returns timestamp values for each sample. */
   public Queue<Double> makeTimestampQueue() {
     Queue<Double> queue = new ArrayBlockingQueue<>(20);
-    SwerveDriveSubsystem.ODOMETRY_LOCK.lock();
+    SwerveDriveIO.ODOMETRY_LOCK.lock();
     try {
       timestampQueues.add(queue);
     } finally {
-      SwerveDriveSubsystem.ODOMETRY_LOCK.unlock();
+      SwerveDriveIO.ODOMETRY_LOCK.unlock();
     }
     return queue;
   }
@@ -106,13 +106,12 @@ public void run() {
       signalsLock.lock();
       try {
         if (IS_CANFD && phoenixSignals.length > 0) {
-          BaseStatusSignal.waitForAll(
-              2.0 / SwerveDriveSubsystem.ODOMETRY_FREQUENCY, phoenixSignals);
+          BaseStatusSignal.waitForAll(2.0 / SwerveDriveIO.ODOMETRY_FREQUENCY, phoenixSignals);
         } else {
           // "waitForAll" does not support blocking on multiple signals with a bus
           // that is not CAN FD, regardless of Pro licensing. No reasoning for this
           // behavior is provided by the documentation.
-          Thread.sleep((long) (1000.0 / SwerveDriveSubsystem.ODOMETRY_FREQUENCY));
+          Thread.sleep((long) (1000.0 / SwerveDriveIO.ODOMETRY_FREQUENCY));
           if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals);
         }
       } catch (InterruptedException e) {
@@ -123,7 +122,7 @@ public void run() {
       }
 
       // Save new data to queues
-      SwerveDriveSubsystem.ODOMETRY_LOCK.lock();
+      SwerveDriveIO.ODOMETRY_LOCK.lock();
       try {
         // Sample timestamp is current FPGA time minus average CAN latency
         //     Default timestamps from Phoenix are NOT compatible with
@@ -148,7 +147,7 @@ public void run() {
           timestampQueue.offer(timestamp);
         }
       } finally {
-        SwerveDriveSubsystem.ODOMETRY_LOCK.unlock();
+        SwerveDriveIO.ODOMETRY_LOCK.unlock();
       }
     }
   }

src/main/java/frc/robot/subsystems/drive/SwerveDriveIO.java

@@ -3,8 +3,13 @@
 import static edu.wpi.first.units.Units.MetersPerSecond;
 
 import com.ctre.phoenix6.CANBus;
+import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.ModuleConfig;
+import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.pathfinding.Pathfinding;
+import com.pathplanner.lib.util.PathPlannerLogging;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -18,14 +23,17 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.generated.TunerConstants;
+import frc.robot.util.LocalADStarAK;
 import frc.robot.util.TimestampedVisionUpdate;
 import java.util.List;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
+import org.littletonrobotics.junction.Logger;
 
 public interface SwerveDriveIO extends Subsystem {
 
@@ -87,6 +95,31 @@ static Translation2d[] getModuleTranslations() {
     };
   }
 
+  default void configure() {
+
+    AutoBuilder.configure(
+        this::getPose,
+        this::setPose,
+        this::getChassisSpeeds,
+        this::runVelocity,
+        new PPHolonomicDriveController(
+            new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)),
+        PP_CONFIG,
+        () ->
+            DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue)
+                == DriverStation.Alliance
+                    .Red, // this is correct, model all trajectories for blue side in path planner
+        this);
+    Pathfinding.setPathfinder(new LocalADStarAK());
+    PathPlannerLogging.setLogActivePathCallback(
+        activePath ->
+            Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[0])));
+    PathPlannerLogging.setLogTargetPoseCallback(
+        targetPose -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
+  }
+
+  ChassisSpeeds getChassisSpeeds();
+
   Transform2d getVelocity();
 
   @Override

src/main/java/frc/robot/subsystems/drive/SwerveDriveSubsystem.java → src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java

@@ -3,11 +3,6 @@
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.Volts;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-import com.pathplanner.lib.pathfinding.Pathfinding;
-import com.pathplanner.lib.util.PathPlannerLogging;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -28,13 +23,12 @@
 import frc.robot.Constants;
 import frc.robot.Constants.Mode;
 import frc.robot.generated.TunerConstants;
-import frc.robot.util.LocalADStarAK;
 import frc.robot.util.TimestampedVisionUpdate;
 import java.util.List;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-public class SwerveDriveSubsystem implements SwerveDriveIO {
+public class SwerveDriveReal implements SwerveDriveIO {
 
   private final SysIdRoutine sysId;
   private final GyroIO gyroIO;
@@ -43,7 +37,7 @@ public class SwerveDriveSubsystem implements SwerveDriveIO {
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
   private final Field2d field2d = new Field2d();
 
-  public SwerveDriveSubsystem(
+  public SwerveDriveReal(
       GyroIO gyroIO,
       ModuleIO flModuleIO,
       ModuleIO frModuleIO,
@@ -61,26 +55,7 @@ public SwerveDriveSubsystem(
     // Start odometry thread
     PhoenixOdometryThread.getInstance().start();
 
-    // Configure AutoBuilder for PathPlanner
-    AutoBuilder.configure(
-        this::getPose,
-        this::setPose,
-        this::getChassisSpeeds,
-        this::runVelocity,
-        new PPHolonomicDriveController(
-            new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)),
-        PP_CONFIG,
-        () ->
-            DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue)
-                == DriverStation.Alliance
-                    .Red, // this is correct, model all trajectories for blue side in path planner
-        this);
-    Pathfinding.setPathfinder(new LocalADStarAK());
-    PathPlannerLogging.setLogActivePathCallback(
-        activePath ->
-            Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[0])));
-    PathPlannerLogging.setLogTargetPoseCallback(
-        targetPose -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
+    configure(); // Configure AutoBuilder and PathPlanner
 
     // Configure SysId
     sysId =
@@ -260,8 +235,9 @@ private SwerveModulePosition[] getModulePositions() {
   }
 
   /** Returns the measured chassis speeds of the robot. */
+  @Override
   @AutoLogOutput(key = "SwerveChassisSpeeds/Measured")
-  private ChassisSpeeds getChassisSpeeds() {
+  public ChassisSpeeds getChassisSpeeds() {
     return kinematics.toChassisSpeeds(getModuleStates());
   }