Swerve Drive Example

Overview

A swerve drivetrain is the largest subsystem on most robots and the one you are least likely to write from scratch. Both major vendors publish a template, and the sensible move is to start from one and understand it rather than reinvent it.

This walkthrough follows the structure of the AdvantageKit swerve template (Team 6328), which builds on Phoenix 6’s Tuner X generator.

The pieces

TunerConstants Comes From Tuner X

TunerConstants.java is produced by the Phoenix Tuner X Swerve Project Generator. You do not edit it by hand and you do not derive its numbers yourself — the generator walks you through the hardware and writes the file.

// Generated by the 2026 Tuner X Swerve Project Generator
// 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.
    private static final Slot0Configs steerGains = new Slot0Configs()
        .withKP(100).withKI(0).withKD(0.25)
        .withKS(0.1).withKV(2.49).withKA(0)
        .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);

    private static final Slot0Configs driveGains = new Slot0Configs()
        .withKP(2.0).withKI(0).withKD(0)
        .withKS(0.1723).withKV(0.7061);

    private static final Current kSlipCurrent = Amps.of(90);

Everything downstream reads from it — Drive computes its drive base radius from the generated module locations, and PathPlanner’s robot config uses the generated wheel radius and gear ratio.

One Module, Four Times

Each corner gets an IO implementation handed its slice of TunerConstants:

drive = new Drive(
    new GyroIOPigeon2(),
    new ModuleIOTalonFX(TunerConstants.FrontLeft),
    new ModuleIOTalonFX(TunerConstants.FrontRight),
    new ModuleIOTalonFX(TunerConstants.BackLeft),
    new ModuleIOTalonFX(TunerConstants.BackRight));

Drive wraps each in a Module that knows its index:

public Drive(
    GyroIO gyroIO,
    ModuleIO flModuleIO,
    ModuleIO frModuleIO,
    ModuleIO blModuleIO,
    ModuleIO brModuleIO) {
  this.gyroIO = gyroIO;
  modules[0] = new Module(flModuleIO, 0, TunerConstants.FrontLeft);
  modules[1] = new Module(frModuleIO, 1, TunerConstants.FrontRight);
  modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft);
  modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight);

The index separates the log paths — Drive/Module0, Drive/Module1, and so on. The array order is fixed as FL, FR, BL, BR and must match what WPILib’s kinematics expects; swapping two entries produces a robot that drives in confidently wrong directions.

Because the constructor takes interfaces rather than concrete hardware, the same Drive runs in simulation and log replay with no changes — see Hardware Abstraction with IO Layers.

Per-Module Work

public void periodic() {
  io.updateInputs(inputs);
  Logger.processInputs("Drive/Module" + Integer.toString(index), inputs);

  int sampleCount = inputs.odometryTimestamps.length;
  odometryPositions = new SwerveModulePosition[sampleCount];
  for (int i = 0; i < sampleCount; i++) {
    double positionMeters = inputs.odometryDrivePositionsRad[i] * constants.WheelRadius;
    odometryPositions[i] = new SwerveModulePosition(positionMeters, inputs.odometryTurnPositions[i]);
  }

  driveDisconnectedAlert.set(!inputs.driveConnected);
  turnDisconnectedAlert.set(!inputs.turnConnected);
}

/** Runs the module with the specified setpoint state. Mutates the state to optimize it. */
public void runSetpoint(SwerveModuleState state) {
  state.optimize(getAngle());
  state.cosineScale(inputs.turnPosition);
  // apply setpoints to the IO layer
}

optimize avoids rotating a module more than 90° by reversing the drive direction instead. cosineScale reduces drive output while the module is still turning, so the robot does not lurch sideways at the start of a motion.

The Alert calls put a disconnected module in front of the drive team instead of leaving it as mysteriously wrong behaviour.

The Odometry Thread

// Start odometry thread
PhoenixOdometryThread.getInstance().start();

Odometry accuracy depends on how often module positions are sampled. The main loop runs at 50 Hz, which is fine for controlling a mechanism but coarse for integrating wheel positions while the robot moves quickly.

A dedicated odometry thread samples the drive and turn signals at a higher rate and buffers them with timestamps. That is why the module inputs carry arrays:

public double[] odometryTimestamps = new double[] {};
public double[] odometryDrivePositionsRad = new double[] {};
public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};

Each loop delivers several samples rather than one, and Drive feeds all of them into the pose estimator with their real timestamps. They are ordinary logged inputs, so this replays like everything else.

Path Following

Drive configures PathPlanner in its own constructor, keeping drivetrain concerns out of RobotContainer:

AutoBuilder.configure(
    this::getPose,
    this::setPose,
    this::getChassisSpeeds,
    this::runVelocity,
    new PPHolonomicDriveController(
        new PIDConstants(translationalAutoP, 0.0, 0.0),
        new PIDConstants(rotationalAutoP, 0.0, 0.0)),
    PP_CONFIG,
    () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
    this);
Pathfinding.setPathfinder(new LocalADStarAK());

PP_CONFIG is built from the generated constants plus the robot’s mass and moment of inertia. Full detail in PathPlanner.

Driving It

drive.setDefaultCommand(
    DriveCommands.joystickDrive(
        drive,
        () -> -controller.getLeftY(),
        () -> -controller.getLeftX(),
        () -> controller.getRightX()));

Axes are passed as suppliers so the command reads fresh joystick positions every loop. The negations on the Y axes are because joysticks report forward as negative.

A drivetrain is the one subsystem where a static factory class (DriveCommands) beats methods on the subsystem — the commands take several suppliers and there are enough of them to be worth their own file.

Characterization Routines

Add these to the auto chooser so they can be run from the dashboard like any auto:

autoChooser.addOption(
    "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive));
autoChooser.addOption(
    "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive));
autoChooser.addOption(
    "Drive SysId (Quasistatic Forward)", drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
autoChooser.addOption(
    "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));

Run these on a clear floor with plenty of space. They drive the robot with no human in the loop, and the dynamic tests accelerate hard.

Resources