Mini-Project: A Teleop Swerve Drive Subsystem

Here we wire WPILib's swerve primitives into a drivable teleop subsystem. The core data flow is: joystick inputs -> ChassisSpeeds -> SwerveDriveKinematics -> four SwerveModuleStates -> per-module motors.

Define the geometry

Kinematics needs the location of each module relative to the robot center. For a 28 in x 28 in frame the wheels sit ~0.29 m from center on each axis:

private static final double X = 0.29, Y = 0.29; // meters
private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
    new Translation2d( X,  Y),  // front-left
    new Translation2d( X, -Y),  // front-right
    new Translation2d(-X,  Y),  // back-left
    new Translation2d(-X, -Y)); // back-right

Drive method

Convert joystick demand to chassis speeds (field-relative using the gyro), run kinematics, desaturate so no module is commanded faster than physically possible, then optimize each state so a module never rotates more than 90 degrees:

public void drive(double vx, double vy, double omega) {
  ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
      vx, vy, omega, m_gyro.getRotation2d());

  SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(speeds);
  SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeedMps);

  for (int i = 0; i < 4; i++) {
    states[i].optimize(m_modules[i].getAngle()); // 2025 instance method
    m_modules[i].setDesiredState(states[i]);
  }
}

Note the 2025 API change: SwerveModuleState.optimize(currentAngle) is now an instance method that mutates the state in place, replacing the deprecated static SwerveModuleState.optimize(state, angle). (WPILib 2025 also added an instance cosineScale() method on the same class.)

The default command

Driving is the drivetrain's default behavior, so bind it as the default command -- and remember the rule that a default command must require its subsystem (the run() factory does this automatically):

m_swerve.setDefaultCommand(m_swerve.run(() ->
    m_swerve.drive(
        -MathUtil.applyDeadband(m_driver.getLeftY(), 0.1) * kMaxSpeedMps,
        -MathUtil.applyDeadband(m_driver.getLeftX(), 0.1) * kMaxSpeedMps,
        -MathUtil.applyDeadband(m_driver.getRightX(), 0.1) * kMaxOmega)));

Verify before you trust it

Publish your module states to NetworkTables and open AdvantageScope's Swerve tab, which renders each module's velocity vector, the chassis speeds, and robot rotation. Push the stick forward: all four arrows should point forward and grow together. Rotate: the arrows form a pinwheel tangent to the frame. If one module points the wrong way, its CANcoder offset or motor inversion is wrong -- fix it before driving, not on the field. This visual check catches the single most common swerve bug (a mis-zeroed module) in seconds.