How to Program FRC Swerve Drive with WPILib

Swerve looks intimidating in code, but the math is already written for you. WPILib ships the kinematics, odometry, and pose estimation as a small set of well-tested classes. Your job is mostly plumbing: turn joystick numbers into a ChassisSpeeds, hand that to a kinematics object, and push the resulting per-module speed-and-angle setpoints into four closed-loop controllers. The "hard" part — the trig that splits robot motion into four independently-steered wheels — is a single method call.

This guide walks the full pipeline in Java: geometry setup, joystick-to-module conversion, field-relative control, module optimization, subsystem structure, and where PathPlanner and Choreo plug in. Every API is checked against the current WPILib docs — but WPILib changes a little every season, so confirm signatures against the version your team runs. For the concept-level picture of how a swerve drivetrain moves, read Swerve Drive Explained first.

The control pipeline, end to end

Hold the whole flow in your head first. Every loop of your robot code (roughly every 20 ms) turns three driver numbers — forward, sideways, and rotational velocity — into a ChassisSpeeds, splits that into four wheel commands, and separately reads the wheels and gyro back to track position:

That is the entire mental model. The rest of this article fills in each box with correct code.

Set up the geometry: SwerveDriveKinematics

SwerveDriveKinematics is the object that knows your robot's shape. You construct it once with each module's location measured from the robot's center, as a Translation2d in meters. WPILib's convention is that positive x points toward the front of the robot and positive y points toward the left.

// +x = front, +y = left; measured from robot center to each module, in meters.
Translation2d frontLeft  = new Translation2d(0.31, 0.31);
Translation2d frontRight = new Translation2d(0.31, -0.31);
Translation2d backLeft   = new Translation2d(-0.31, 0.31);
Translation2d backRight  = new Translation2d(-0.31, -0.31);

SwerveDriveKinematics kinematics =
    new SwerveDriveKinematics(frontLeft, frontRight, backLeft, backRight);

The order you pass modules here is the order everything else uses — the array from toSwerveModuleStates and the SwerveModulePosition[] you feed odometry must all match it. Pick one order and keep it identical everywhere. A swapped index is the single most common "my robot spins in a circle" bug.

You do not need millimeter-perfect measurements, but a square robot with modules equidistant from center keeps the math symmetric. If you are still choosing hardware, comparing swerve modules is worth doing before locking in a wheelbase.

From joystick to module states

The bridge between "the driver pushed the stick forward" and "each wheel should do X" is ChassisSpeeds. It holds three fields — vxMetersPerSecond (forward), vyMetersPerSecond (sideways/left), and omegaRadiansPerSecond (angular) — which the three-argument constructor takes in that order. Positive vy moves the robot left; positive omega is a counterclockwise turn.

// vx forward, vy left, omega counterclockwise
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 0.0, 0.5);

SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
// states are returned in your construction order (front-left, front-right, ...)

Each SwerveModuleState carries a speedMetersPerSecond and an angle (a Rotation2d). That is all toSwerveModuleStates gives you — the inverse-kinematics trig, already solved.

Desaturate so the math stays physical

When the driver asks for full translation and rotation at once, the kinematics can return a wheel speed no motor can deliver. Clamping just that wheel makes the robot drive crooked; instead, scale every wheel down proportionally so the fastest sits at your max speed. WPILib does this for you:

// Scales all four speeds down together if any exceeds the cap.
SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_SPEED_MPS);

desaturateWheelSpeeds (renamed from normalizeWheelSpeeds in 2022) takes the state array and your attainableMaxSpeedMetersPerSecond. Call it every loop, right after toSwerveModuleStates. MAX_SPEED_MPS should be the module's real free speed at the wheel for your gear ratio — use the vendor's published figure, not a guess.

Field-relative vs robot-relative

Robot-relative means "forward" is wherever the robot faces. Field-relative means "forward" is always downfield regardless of heading — which is what drivers actually want. WPILib converts field-relative intent into a robot-frame ChassisSpeeds using your gyro angle:

ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
    xMetersPerSecond,       // downfield, field frame
    yMetersPerSecond,       // to the left, field frame
    omegaRadPerSecond,
    gyro.getRotation2d());  // robot's current heading

For this to behave, the gyro must agree with the field. So you need a "reset heading" button that zeroes the gyro when the robot points downfield (to re-orient after a collision), and you must handle the fact that "downfield" flips with alliance color — many teams add 180 degrees for the red alliance. Because alliance conventions and field layout change every year, check the current game manual rather than hard-coding last season's assumptions.

Make modules efficient: optimize and cosine scaling

A swerve module never needs to steer more than 90 degrees: if a wheel sits at 170 and you command 10, it is cheaper to steer to 190 and reverse the drive direction. SwerveModuleState.optimize does exactly that, mirroring the target angle and negating the speed when that shortens the turn.

// Legacy static form — returns a new, optimized state:
Rotation2d currentAngle = getSteerAngle();
SwerveModuleState optimized = SwerveModuleState.optimize(desired, currentAngle);

WPILib 2025 added an instance optimize (and a cosineScale method) to SwerveModuleState. The instance form mutates the state in place instead of returning a new one:

// WPILib 2025+ instance form — mutates 'desired' in place:
desired.optimize(currentAngle);

Both do the same thing; use whichever your season's API exposes. Either way, optimize needs the module's current measured angle — read it from your steering encoder, not the last commanded value.

An optional refinement is cosine compensation: while a module is still rotating toward its target, scale its drive speed by the cosine of the angle error so it does not fight sideways during the swing. The WPILib docs show it as:

optimized.speedMetersPerSecond *= optimized.angle.minus(currentAngle).getCos();

Also useful: ChassisSpeeds.discretize corrects the sideways drift a robot picks up when it translates and rotates at once under discrete 20 ms updates.

Structure the code: module subsystem + drive subsystem

Swerve code stays sane when you split it into two layers. A SwerveModule class wraps one physical module and hides its motor controllers and encoders. A Drive subsystem owns the four modules, the kinematics, the gyro, and the odometry. This mirrors command-based programming: the subsystem is the hardware boundary, commands sit on top.

Keep vendor-specific motor calls behind the module's methods so the rest of your code only speaks WPILib types:

public class SwerveModule {
    // Report the module's live velocity + angle (for closed-loop / telemetry).
    public SwerveModuleState getState() {
        return new SwerveModuleState(getDriveVelocityMps(), getSteerAngle());
    }

    // Report accumulated distance + angle (for odometry).
    public SwerveModulePosition getPosition() {
        return new SwerveModulePosition(getDriveDistanceMeters(), getSteerAngle());
    }

    // Accept a target and drive the motors to it.
    public void setDesiredState(SwerveModuleState desired) {
        desired.optimize(getSteerAngle());
        // Feed desired.speedMetersPerSecond to your drive motor's velocity loop and
        // desired.angle to your steer motor's position loop (vendor API varies).
    }
}

The drive subsystem ties them together with one clean entry point for both teleop and autonomous:

public void drive(ChassisSpeeds robotRelativeSpeeds) {
    SwerveModuleState[] states = kinematics.toSwerveModuleStates(robotRelativeSpeeds);
    SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_SPEED_MPS);
    for (int i = 0; i < modules.length; i++) {
        modules[i].setDesiredState(states[i]);
    }
}

That drive(ChassisSpeeds) method is deliberately the only way motion reaches the wheels — teleop and autonomous both go through it, so there is one place to debug.

Track position: odometry and pose estimation

Kinematics answers "how do I move"; odometry answers "where am I now." SwerveDriveOdometry integrates your module positions and gyro heading into a Pose2d — the robot's x, y, and rotation on the field. Its constructor takes the kinematics, gyro angle, a SwerveModulePosition array (in module order), and optionally a starting pose.

SwerveDriveOdometry odometry = new SwerveDriveOdometry(
    kinematics,
    gyro.getRotation2d(),
    getModulePositions());   // SwerveModulePosition[] in construction order

@Override
public void periodic() {
    odometry.update(gyro.getRotation2d(), getModulePositions());
}

public Pose2d getPose() {
    return odometry.getPoseMeters();
}

Odometry consumes SwerveModulePosition (accumulated distance + angle), not SwerveModuleState (velocity + angle), and needs it updated every loop — so call update in periodic().

Pure odometry drifts. Relying only on encoders and gyro, it accumulates error from robot contact, wheel scrub, and small calibration mistakes. It is usually very accurate during the short autonomous period, but by late teleop the estimate can be off by a meter or more. When you reset the gyro or encoders, call resetPosition(gyroAngle, modulePositions, newPose) so odometry does not read the jump as real motion.

Fusing vision with SwerveDrivePoseEstimator

If you add a vision system (AprilTag cameras, say), swap SwerveDriveOdometry for SwerveDrivePoseEstimator. It is a drop-in replacement with the same update pattern that also fuses latency-compensated field-relative pose measurements from vision to correct encoder drift.

poseEstimator.update(gyro.getRotation2d(), getModulePositions());

// When a camera reports a field-relative pose with a timestamp:
poseEstimator.addVisionMeasurement(visionPose, timestampSeconds);

The estimator blends sources by standard deviation: a smaller deviation means "trust this more." Give jittery vision a larger deviation so a noisy frame does not yank your pose across the field. For deeper coverage of the estimator and tuning those deviations, see FRC odometry and pose estimation.

Where PathPlanner and Choreo plug in

Once teleop works and odometry is trustworthy, autonomous path-following is mostly configuration. Both PathPlanner and Choreo generate a trajectory offline, then at runtime sample it into a ChassisSpeeds and hand that straight to the drive(ChassisSpeeds) method you already built. Your subsystem does not change.

PathPlanner centers on AutoBuilder. You configure it once, usually in the drive subsystem constructor, wiring it to your pose supplier, pose reset method, robot-relative speeds supplier, and drive output:

AutoBuilder.configure(
    this::getPose,                 // Supplier<Pose2d>
    this::resetPose,               // Consumer<Pose2d>
    this::getRobotRelativeSpeeds,  // Supplier<ChassisSpeeds>
    (speeds, feedforwards) -> driveRobotRelative(speeds),
    new PPHolonomicDriveController(
        new PIDConstants(5.0, 0.0, 0.0),   // translation PID
        new PIDConstants(5.0, 0.0, 0.0)),  // rotation PID
    config,                        // RobotConfig (often loaded from GUI settings)
    () -> DriverStation.getAlliance()
              .filter(a -> a == Alliance.Red).isPresent(),  // flip path for red
    this);                         // the drive subsystem

Note AutoBuilder.configure with a PPHolonomicDriveController — the current API, replacing the older configureHolonomic from pre-2025 tutorials.

Choreo takes a time-optimal approach: its solver produces trajectories that respect your robot's real mass, moment of inertia, and motor limits. In robot code, ChoreoLib's Java AutoFactory builds commands from saved trajectories — trajectoryCmd() returns a command following a named trajectory, and a SwerveSample is one sampled instant you feed into your drive method. Choreo favors small, reusable "segmented" trajectories over one monolithic path.

PathPlanner also offers a SwerveSetpointGenerator that limits module acceleration and steering so wheel forces stay under the friction limit — handy for preventing slip on aggressive paths. Which tool to reach for is its own decision; Choreo vs PathPlanner compares them head to head. For programming, the takeaway is the same: because both emit a ChassisSpeeds, a clean drive subsystem makes either one a small integration.

A sane bring-up order

Do not wire everything and hope. Bring swerve up in layers, testing each before the next:

1. One module at a time. Command a fixed angle and a positive speed; confirm the wheel points and drives where you expect. Fix any inverted drive or steer motor now.
2. Steering offsets. Set each module's absolute-encoder offset so "zero" means the wheel points straight ahead. Every downstream calculation assumes this.
3. Robot-relative motion. Push a small vx with zero rotation — all four wheels forward, robot tracks straight. Then check vy crab-walks sideways.
4. Rotate in place. Command only omega; the wheels should form a circle and the robot should spin with no translation.
5. Field-relative. Reset the gyro downfield, then confirm "stick forward" always drives downfield regardless of heading.
6. Odometry sanity. Drive a square and confirm getPose returns near the origin.

Only once odometry is trustworthy should you layer PathPlanner or Choreo on top — a path-follower built on drifty odometry chases a phantom position and looks broken when the real fault is upstream.

Frequently asked questions

Do I have to write the swerve math myself?

No. SwerveDriveKinematics already implements the forward and inverse kinematics. You provide module locations and a ChassisSpeeds; toSwerveModuleStates returns the per-module speeds and angles. You never write the trig by hand.

What is the difference between SwerveModuleState and SwerveModulePosition?

SwerveModuleState holds a wheel velocity and angle and commands motion. SwerveModulePosition holds accumulated distance and angle and lets odometry track where the robot has gone. Use states to drive, positions to localize.

Why does my robot spin when I command straight-line driving?

Almost always a module-ordering mismatch or a bad steering-encoder offset — not the kinematics. Confirm the Translation2d order, the states array, and the SwerveModulePosition[] you feed odometry are all in the same order, then verify each module's zero offset.

How do I make driving field-relative?

Build your ChassisSpeeds with ChassisSpeeds.fromFieldRelativeSpeeds(vx, vy, omega, gyroAngle) and keep the gyro calibrated to the field. Add a button that zeroes the gyro when the robot faces downfield, and flip the "downfield" definition by alliance color per the current game manual.

Should I use SwerveDriveOdometry or SwerveDrivePoseEstimator?

Start with SwerveDriveOdometry — simpler, and very accurate over the short autonomous period. Move to SwerveDrivePoseEstimator when you add a vision system, since it fuses latency-compensated vision measurements to correct the drift that accumulates over a full match.

Do I need PathPlanner or Choreo to run swerve?

No. Both are autonomous path-followers that sit on top of a working swerve drive, emitting a ChassisSpeeds into the same drive method your teleop uses. Get teleop and odometry solid first, then add either as a thin integration.

Swerve programming rewards a clean pipeline more than clever code. Keep one path from intent to wheels, keep your module ordering consistent, and let WPILib's kinematics and odometry classes do the heavy lifting. Once that spine is solid, everything from field-relative driving to full autonomous routines becomes an incremental add rather than a rewrite. To go deeper on any stage, the guides library breaks each one down further.