Understanding and Coding a Swerve Module in FRC

Swerve drive is one of the most flexible and rewarding drivetrain systems in FRC. It allows your robot to move in any direction — forward, sideways, diagonally — while keeping its orientation fixed or rotating independently. That control comes at the cost of complexity: four independently steered modules, each with its own motor pair, encoder, and PID logic.

In this post, we’ll walk through the foundation of a swerve module implementation in WPILib using CTRE’s Phoenix 6 API. By the end, you should understand not only how to code a swerve module, but why each piece matters. This article is written both for rookie programmers and experienced teams who want a clean, modular reference.

The Idea Behind Swerve

A traditional tank drive uses two sides of motors: one for the left, one for the right. To turn, one side spins faster than the other. This works well, but it means your robot can’t move sideways without turning first.

Swerve drive changes that completely. Instead of linking wheels in pairs, each wheel (or module) can rotate independently to face any direction and drive at any speed.

So what does that enable?

• The robot can move in any direction instantly — forwards, sideways, diagonally.
• It can rotate and translate at the same time, something impossible in tank drive.
• It makes autonomous paths smoother and more efficient, since motion is continuous rather than segmented.

Mathematically, this is made possible by vector addition. Each wheel’s motion contributes a velocity vector. By combining all four vectors, the robot can produce any movement it wants — translation, rotation, or both simultaneously.

Each swerve module contains the following:

1. Turning motor
2. Driving motor
3. External encoder

It makes a movement like this possible:

Screen recorded from PathPlanner GUI

Defining Constants

Before writing any logic, we define our constants. These govern tuning parameters, motor calibration, and physical conversions. Keeping them centralized makes it easier to iterate later.

public class Constants {
    public static final double kPModuleTurningController = 0.5;
    public static final double kPModuleDriveController = 0;

    public static final double kMaxSpeedMetersPerSecond = 4.0;
    // TODO: 'EncoderDistancePerPulse' should be calculated based on the gearing and wheel diameter
    public static final double kDriveEncoderDistancePerPulse = 0.0011265396; 

    public static final String CAN_BUS_NAME = "CANivore";
}

kPModuleTurningController: Controls how aggressively the module turns toward its target angle.

kPModuleDriveController: Starts at zero; increase later when tuning drive responsiveness.

The kDriveEncoderDistancePerPulse constant translates motor encoder readings into meters or inches traveled, you can change the units you use by changing this constant too — just try to keep everything consistent. You’ll need to calculate this using your wheel diameter and gear reduction.

Finally, specifying the CAN bus name (“CANivore” in this example) helps WPILib locate your devices if you use multiple buses. If you are not using any additional CAN bus and directly connect it to roborio you don’t need this constant.

Constructing the Swerve Module

Now we move into the SwerveModule class, which is what we will use for the rest of the tutorial. There are 4 swerve modules in each robot, so we wanted to have a unified way of controlling these motors. SwerveModule class allows just that.

Here is the field of the class where we initialize our core motors and controllers:

public class SwerveModule {
  private final TalonFX m_driveMotor;
  private final TalonFX m_turningMotor;
  private final AnalogInput m_turningEncoderInput;

  private double turningMotorOffsetRadians;
  private double m_driveMotorGain;

  private final PIDController m_drivePIDController =
      new PIDController(Constants.kPModuleDriveController, 0, 0);
  private final PIDController m_turningPIDController =
      new PIDController(Constants.kPModuleTurningController, 0, 0.0001);
}

We first set our two motors — one for forward/backward and one for turning. In our case, these are TalonFX, but it is also possible to use SparkMax or other motors by changing some of the code. AnalogInput is used as the encoder; it is an external encoder, not the built-in encoder — more on that later in the post.

Then the turningMotorOffsetRadians and m_driveMotorGain are both values that need to be tuned to actually make the front of the robot front — eg, the 0 degrees of the swerve module might be facing some diagonal direction, so we want to add an offset to zero it to the direction that we desire.

PIDController for driving is more about improving accuracy and having a smoother drive; however, it is not required. On the other hand, the PIDConstroller for turning is something you need to use and fine-tune to get better results in rotation, which would significantly impact the speed of the robot.

Initialization and Setup

Inside the constructor, we initialize hardware and configure basic parameters:

public SwerveModule(
      int driveMotorChannel,
      int turningMotorChannel,
      int analogEncoderPort,
      double turningMotorOffsetRadians,
      double driveMotorGain
      ) {

    m_driveMotor = new TalonFX(driveMotorChannel, new CANBus(Constants.CAN_BUS_NAME));
    m_turningMotor = new TalonFX(turningMotorChannel,  new CANBus(Constants.CAN_BUS_NAME));

    m_driveMotor.setNeutralMode(NeutralModeValue.Brake);
    m_turningMotor.setNeutralMode(NeutralModeValue.Brake);

    this.turningMotorOffsetRadians = turningMotorOffsetRadians;

    m_driveMotorGain = driveMotorGain;

    m_turningEncoderInput = new AnalogInput(analogEncoderPort);

    m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
}

We start off by initializing our two motors — again, you may not need the second parameter if you are directly connecting the motors to RoboRio.

We set the mode of both motors to Brake as we want to be able to quickly change direction and move.

We set the tuning variables to the parameters. And initialize the encoder input with the given port.

Why enableContinuousInput() Matters

A PID controller normally drives the error between the current and target values. Without continuous input, going from 0° to 360° would make the motor rotate a full revolution — even though those angles are equivalent.

By calling:

m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);

We tell the controller that -π and π represent the same point, allowing it to take the shortest path between two angles.

Left is how the PID views the value before continuous input, and right is how it views the values after continuous input.

Note: WPILib’s SwerveModuleState also uses radians, so keeping everything in radians avoids unnecessary conversions and rounding errors.

Getting the Module’s State

Before commanding motion, you need a way to know what each module is doing right now — its current wheel angle and drive velocity. That’s where the following helper methods come in:

public SwerveModuleState getState() {
    return new SwerveModuleState(
        m_driveMotor.getVelocity().getValueAsDouble(),
        new Rotation2d(getTurningEncoderRadians())
    );
}

public SwerveModulePosition getPosition() {
    return new SwerveModulePosition(
        m_driveMotor.getPosition().getValueAsDouble() * Constants.kDriveEncoderDistancePerPulse,
        new Rotation2d(getTurningEncoderRadians())
    );
}

• getState() returns the instantaneous velocity and orientation of the wheel — this is what the kinematics class uses to estimate robot motion in real time.
• getPosition() reports the total distance traveled and current orientation, which odometry uses for tracking the robot’s pose across the field.

See how both of them use getTurningEncoderRadians() which converts the analog voltage from the encoder into a meaningful angle:

public double getTurningEncoderRadians(){
    double angle = (1.0 - (m_turningEncoderInput.getVoltage()/RobotController.getVoltage5V())) * 2.0 * Math.PI + turningMotorOffsetRadians;
    angle %= 2.0 * Math.PI;
    if (angle < 0.0) {
        angle += 2.0 * Math.PI;
    }

    return angle;
}

To summarize, the encoder returns a value — in volts — between 0V and 5V. We can use this to map out a full rotation, as there is a linear relationship between encoder value and the angle. We can divide this encoder value by 5V to get a range 0–1, and that would correspond to 0–2pi rad. So we just multiply it by 2pi and add turningMotorOffsetRadians, depending on the front of the robot and the zero position of the encoder.

An important note here is that subtracting from 1 in the equation depends on the placement of the encoder, so you may want to see if the change in angle makes sense and if it is reversed, just remove “1 -”.

Commanding the Module — setDesiredState()

This is the core function of your SwerveModule class. It takes in a target velocity and angle (from WPILib’s SwerveModuleState) and drives the motors to achieve that.

public void setDesiredState(SwerveModuleState desiredState) {
    SwerveModuleState state = desiredState;

    // Prevent jittering at very low speeds
    if (Math.abs(state.speedMetersPerSecond) < 0.001) {
        stop();
        return;
    }

    state.optimize(new Rotation2d(getTurningEncoderRadians()));

    final double driveOutput = m_drivePIDController.calculate(
        m_driveMotor.getVelocity().getValueAsDouble(),
        state.speedMetersPerSecond
    );

    final double driveFeedForward = state.speedMetersPerSecond / Constants.kMaxSpeedMetersPerSecond;

    final double turnOutput = m_turningPIDController.calculate(
        getTurningEncoderRadians(),
        state.angle.getRadians()
    );

    m_driveMotor.set(
        MathUtil.clamp((driveOutput + driveFeedForward) * m_driveMotorGain, -1.0, 1.0)
    );
    m_turningMotor.set(turnOutput);
}

Let’s unpack this:

Preventing Jitter

When the desired speed is extremely small (less than 0.1% of full power), the module shouldn’t bother rotating to match a new angle. That would only cause the wheel to twitch in place.

So we check:

if (Math.abs(state.speedMetersPerSecond) < 0.001) {
    stop();
    return;
}

This line saves wear on your steering motor and reduces noise when the robot is idle.

Optimizing the Wheel Angle

state.optimize(new Rotation2d(getTurningEncoderRadians()));

SwerveModuleState.optimize() ensures the module takes the shortest way to reach the target direction. For example, instead of rotating 270°, it might flip the wheel 90° and reverse drive direction — same motion, faster response.

See the illustration below:

Illustration on the left rotates more compared to the right illustration to be in the same position.

Drive Output Calculation

The drive motor’s control has two parts:

First, PID drives the motor to the target speed by using the previously defined proportional, integral, and derivative values. (See more on how PID works):

m_drivePIDController.calculate(currentVelocity, targetVelocity)

Then we use feedforward to keep the wheel moving at the same velocity:

state.speedMetersPerSecond / Constants.kMaxSpeedMetersPerSecond

Both are combined, scaled by m_driveMotorGain, and clamped between -1 and 1 (full reverse to full forward).

Turning Output Calculation

Turning is entirely handled by the PID controller:

final double turnOutput =
    m_turningPIDController.calculate(getTurningEncoderRadians(), state.angle.getRadians());

This minimizes angular error between the current and target direction. Because we previously enabled continuous input, it always takes the shortest path across the ±π boundary.

Setting Motor Outputs

Finally, we command the motors:

m_driveMotor.set(
        MathUtil.clamp(
            (driveOutput + driveFeedForward) * m_driveMotorGain,
             -1.0, // min -100%
             1.0) // max +100%
    );
    m_turningMotor.set(turnOutput);

At this point, each module runs semi-independently: it continuously reads its encoder, calculates new outputs, and steers to the desired state.

Putting It All Together

With SwerveModule complete, you can now instantiate four of them in a drivetrain subsystem:

SwerveModule frontLeft = new SwerveModule(...);
SwerveModule frontRight = new SwerveModule(...);
SwerveModule backLeft = new SwerveModule(...);
SwerveModule backRight = new SwerveModule(...);

Then, using SwerveDriveKinematics, combine their states into a robot-level motion command. WPILib’s kinematics and odometry classes handle the math — your SwerveModule executes per-wheel control.

Next Steps:

After this point, you need to perform the calculations to enable these separate modules to work together in moving the robot. This could be done in a wide variety of ways, depending on how you want to organize everything, but just make sure to keep your units consistent.

You can watch this video to go deeper into swerve drive and how to code it.

Here is the complete code of the SwerveModule:

GitHub - SuleymanSade/SwerveDriveBase

I write about learning AI by building real tools, not just tutorials.

---

I write more tutorials, personal thoughts and development stories on my blogs you can:

• 📰 Read more of my posts on Medium
• 💬 Let’s connect on LinkedIn

Thanks for reading — let me know if you’d like to collaborate on projects similar to these or have any questions.