Unit 7 · Lesson 5

The SwerveModule Class

This is the lesson where everything converges. The hardware configuration from Lesson 2, the optimize() call from Lesson 3, and the position data for Lesson 4's odometry all live inside one class. The SwerveModule is the heart of the swerve system — and it runs four times every 20 ms.

By the end of this lesson, you will:

Write a complete SwerveModule class that encapsulates a TalonFX drive motor, TalonFX steering motor, and CANcoder
Configure Phoenix 6 onboard PID (Slot 0) on the steering motor with continuous wrap enabled
Configure velocity feedforward on the drive motor using Phoenix 6's VelocityVoltage control request
Implement a complete setDesiredState() method with optimize(), near-zero guard, and unit conversions
Implement getState() and getPosition() to return current module data for odometry and telemetry
Publish per-module telemetry to SmartDashboard and explain which values to watch during first bringup

The SwerveModule Class: One Job, Done Completely

The SwerveModule class is not a WPILib subsystem — it does not extend SubsystemBase. It is a plain Java class, instantiated four times by the SwerveDrivetrain subsystem. Its job is narrow and total: given a desired state (speed + angle), make the physical hardware achieve it as accurately and quickly as possible.

Every public method on this class serves one of two purposes: commanding the module (setDesiredState()) or reading from it (getState(), getPosition()). Nothing else belongs here.

Inputs to SwerveModule

Constructor: CAN IDs, CANcoder offset, drive inversion flag
setDesiredState(SwerveModuleState) — target speed + angle from kinematics

Outputs from SwerveModule

getState() → current SwerveModuleState (actual velocity + angle)
getPosition() → SwerveModulePosition (distance + angle) for odometry
SmartDashboard telemetry in updateTelemetry()

Two Control Problems, Two Strategies

Inside every setDesiredState() call, two independent control problems are solved simultaneously — one for the steering motor and one for the drive motor. They use different strategies because they have different requirements.

Steering: Position PID

The steering motor must reach and hold a precise angle. PID position control is the right tool: the P term applies force proportional to the angular error, the I term eliminates steady-state offset, and the D term damps overshoot. The critical configuration: continuous input wrapping. Without it, going from 179° to −179° triggers a 358° turn. With wrapping enabled, the controller sees this as a 2° transition. In Phoenix 6, this is set as withContinuousWrap(true) on the position control request.

Drive: Velocity Feedforward + P

The drive motor must hold a commanded speed against varying loads. Feedforward + PID velocity control is the right tool. Feedforward (kV, kS) predicts the voltage needed for a given speed — it does 95% of the work instantly, without waiting for error to accumulate. A small proportional gain cleans up the remaining error. This combination produces much smoother, more responsive drive behavior than P-only control. In Phoenix 6, this is expressed as a VelocityVoltage control request with onboard Slot 0 gains and feedforward constants.

Steering PID Behavior Visualizer

Select a scenario to see how the steering PID responds. The purple bar is the current wheel angle, the red marker is the target angle.

Steering PID — position control response

Current angle

0°

Steering output

Angular error

5°

← select a scenario to see how the steering PID responds

Part 1: The Constructor and Hardware Configuration

The constructor accepts the three CAN IDs, the CANcoder offset, and an optional drive inversion flag. It creates hardware objects, applies all Phoenix 6 configuration, and stores Signal handles for fast periodic reads. Click the highlighted tokens.

SwerveModule.java — fields and constructor

public class SwerveModule {

  // ── Hardware ─────────────────────────────────────
  private final TalonFX m_driveMotor;
  private final TalonFX m_steerMotor;
  private final CANcoder m_cancoder;

  // ── Control requests (reused each loop, no GC) ───
  private final VelocityVoltage m_driveRequest = new VelocityVoltage(0).withSlot(0);
  private final PositionVoltage m_steerRequest = new PositionVoltage(0).withSlot(0);

  // ── Signal handles for fast reads ────────────────
  private final StatusSignal<Double> m_driveVelocity;
  private final StatusSignal<Double> m_drivePosition;
  private final StatusSignal<Double> m_steerAngle;

  public SwerveModule(int driveId, int steerId, int encoderId,
                    double encoderOffsetRot, boolean driveInverted) {

    m_driveMotor = new TalonFX(driveId);
    m_steerMotor = new TalonFX(steerId);
    m_cancoder = new CANcoder(encoderId);

    configDriveMotor(driveInverted);
    configSteerMotor();
    configEncoder(encoderOffsetRot);

    // Cache signal handles — avoid repeated API calls
    m_driveVelocity = m_driveMotor.getVelocity();
    m_drivePosition = m_driveMotor.getPosition();
    m_steerAngle = m_cancoder.getAbsolutePosition();

    // Set signal update rates
    BaseStatusSignal.setUpdateFrequencyForAll(100,
      m_driveVelocity, m_drivePosition, m_steerAngle);
  }

  private void configDriveMotor(boolean inverted) {
    TalonFXConfiguration cfg = new TalonFXConfiguration();
    cfg.MotorOutput.withNeutralMode(NeutralModeValue.Brake)
      .withInverted(inverted ? InvertedValue.Clockwise_Positive
                             : InvertedValue.CounterClockwise_Positive);
    cfg.CurrentLimits.withStatorCurrentLimit(Constants.Swerve.kDriveCurrentLimitAmps)
      .withStatorCurrentLimitEnable(true);
    cfg.Feedback.withSensorToMechanismRatio(Constants.Swerve.kDriveGearRatio);
    // Slot 0: velocity PID + feedforward for drive
    cfg.Slot0.withKP(0.1).withKV(0.12).withKS(0.25);
    m_driveMotor.getConfigurator().apply(cfg);
  }

  private void configSteerMotor() {
    TalonFXConfiguration cfg = new TalonFXConfiguration();
    cfg.MotorOutput.withNeutralMode(NeutralModeValue.Brake);
    cfg.CurrentLimits.withStatorCurrentLimit(Constants.Swerve.kSteerCurrentLimitAmps)
      .withStatorCurrentLimitEnable(true);
    cfg.Feedback.withSensorToMechanismRatio(Constants.Swerve.kSteerGearRatio)
      .withRemoteCANcoder(m_cancoder);
    // Slot 0: position PID for steering
    cfg.Slot0.withKP(50.0).withKD(0.5);
    m_steerMotor.getConfigurator().apply(cfg);
  }
}

← click a highlighted token

Part 2: `setDesiredState()` — The Core Method

This is the method the drivetrain calls 50 times per second, once per module. It receives a target state from kinematics, optimizes it, converts units, and issues the two hardware commands. Every word of this method matters.

SwerveModule.java — setDesiredState(), getState(), getPosition()

public void setDesiredState(SwerveModuleState desired) {

  // 1. Read current steering angle
  Rotation2d currentAngle = Rotation2d.fromRotations(
    m_steerAngle.refresh().getValueAsDouble());

  // 2. Optimize — never rotate more than 90°
  SwerveModuleState state =
    SwerveModuleState.optimize(desired, currentAngle);

  // 3. Near-zero guard — prevents jitter at standstill
  if (Math.abs(state.speedMetersPerSecond) < 0.001) {
    m_driveMotor.stopMotor();
    return;
  }

  // 4. Drive: m/s → wheel RPS → set velocity
  double driveRPS = state.speedMetersPerSecond
    / Constants.Swerve.kWheelCircumference;
  m_driveMotor.setControl(m_driveRequest.withVelocity(driveRPS));

  // 5. Steer: Rotation2d → rotations → set position
  double targetRot = state.angle.getRotations();
  m_steerMotor.setControl(m_steerRequest.withPosition(targetRot));
}

// ── Sensor reads for drivetrain / odometry ────────────

public SwerveModuleState getState() {
  return new SwerveModuleState(
    m_driveVelocity.refresh().getValueAsDouble()
      * Constants.Swerve.kWheelCircumference,
    Rotation2d.fromRotations(m_steerAngle.refresh().getValueAsDouble())
  );
}

public SwerveModulePosition getPosition() {
  return new SwerveModulePosition(
    m_drivePosition.refresh().getValueAsDouble()
      * Constants.Swerve.kWheelCircumference,
    Rotation2d.fromRotations(m_steerAngle.refresh().getValueAsDouble())
  );
}

// ── Telemetry — call from drivetrain periodic() ────────
public void updateTelemetry(String prefix) {
  SmartDashboard.putNumber(prefix+"/SteerAngleDeg",
    m_steerAngle.getValueAsDouble() * 360);
  SmartDashboard.putNumber(prefix+"/DriveVelocityMPS",
    m_driveVelocity.getValueAsDouble()*Constants.Swerve.kWheelCircumference);
  SmartDashboard.putNumber(prefix+"/DriveCurrentA",
    m_driveMotor.getStatorCurrent().getValueAsDouble());
}

← click a highlighted token

⚠️ The near-zero guard and the PositionVoltage continuous wrap

Two subtle but critical details: (1) Without the near-zero guard, a 0.001 m/s command still tells the drive motor to spin slowly — and the steering PID will try to hold the angle precisely, causing visible motor activity at a standstill. The guard returns immediately, stopping both motors when the robot isn't supposed to be moving. (2) The PositionVoltage control request must have continuous wrap enabled via m_steerRequest.withEnableFOC(true) and the motor's ClosedLoopGeneral.withContinuousWrap(true) in TalonFXConfiguration. Without it, the steering PID will try to drive through 0°/360° the long way around.

💡 Reuse control request objects — never create them inside the control loop

Notice that m_driveRequest and m_steerRequest are declared as fields, not created inside setDesiredState(). Creating new VelocityVoltage() or new PositionVoltage() 50 times per second generates significant garbage collection pressure. Phoenix 6 is designed to have request objects mutated in place (the .withVelocity() and .withPosition() calls return this, so you can chain them). Declare once as a field, mutate in the loop.

Unit Conversions: The Full Chain

Swerve module code involves multiple unit systems. Getting any conversion wrong produces wrong behavior with no compile error. Here is the complete chain for the drive motor, traced from hardware to code and back:

TalonFX native position unit: Rotations of the motor shaft
After withSensorToMechanismRatio(6.75): Position is now in wheel rotations (motor rotations ÷ 6.75)
getPosition() conversion: Wheel rotations × wheel circumference (meters) = distance in meters
getVelocity() conversion: Wheel RPS × wheel circumference = velocity in m/s
setDesiredState() conversion: Target m/s ÷ wheel circumference = target wheel RPS → passed to VelocityVoltage

The gear ratio is the pivot. With withSensorToMechanismRatio(), you express it once in configuration and every subsequent encoder read is already in wheel rotations. Multiplying by circumference then gives meters. Every conversion in the class reduces to these two multiplications.

First-Bringup Telemetry: What to Watch

When enabling the robot for the first time after building the SwerveModule class, open Shuffleboard and watch these four values for each module. They tell you immediately which categories of problems exist.

🔍 LRI Perspective: "Four numbers, ten minutes"

Every swerve bringup goes through the same first-enable ritual on 2910: publish SteerAngleDeg, DriveVelocityMPS, DriveCurrentA, and desired vs. actual angle. Enable. Push the joystick gently. Watch the four numbers for each module on Shuffleboard. A module that doesn't move its SteerAngleDeg when commanded → steering motor or encoder issue. A module with SteerAngleDeg changing but DriveVelocityMPS stuck at zero → drive motor issue. A module driving in the wrong direction → inversion wrong. A module with SteerAngleDeg oscillating wildly → P gain too high. Four numbers diagnose every first-enable failure in under ten minutes. That diagnostic check takes thirty seconds to set up. Don't skip it.

🔌 System Check — Single-Module Isolation Test

Before testing all four modules together:

Create a test command or use teleopInit() to call setDesiredState(new SwerveModuleState(0, new Rotation2d())) on one module. Verify it holds at 0° without oscillation. If it oscillates, reduce the steer Slot 0 kP gain. If it holds weakly and can be manually deflected, increase kP.
Call setDesiredState(new SwerveModuleState(1.0, new Rotation2d())) on the same module with the robot on blocks. Verify the drive wheel spins forward at approximately 1 m/s (use getState().speedMetersPerSecond on SmartDashboard). If it spins backward, the drive inversion flag is wrong — flip it in the constructor call.
Command steering to 90° (Rotation2d.fromDegrees(90)). The wheel should rotate to the left side of the robot (positive Y direction in WPILib convention). If it rotates the wrong direction, the steering motor or CANcoder may need inversion. Verify in Tuner X which direction produces increasing absolute position.
Check getPosition().distanceMeters increases as the wheel spins forward and decreases as it spins backward. If it behaves oppositely, the drive motor position inversion doesn't match the velocity inversion — both must be consistent.

Knowledge Check

1. You enable the robot and one module's wheel rotates 270° counterclockwise to reach a target that's only 90° clockwise away. What is missing from your code?

A The steer Slot 0 kP gain is too high, causing overshoot to the wrong direction.
B The CANcoder offset is wrong, shifting all angle targets by 270°.
C SwerveModuleState.optimize() is not being called. Without it, the module always takes the raw kinematic angle, which may require traveling more than 90° — optimize() would flip the angle and reverse the drive speed instead.
D The PositionVoltage request's continuous wrap is not enabled, causing the PID to drive through 0°.

2. Your drive motor's TalonFX reports a velocity of 4.44 wheel rotations/second. What is the wheel's speed in m/s? (Wheel circumference = 0.319 m. withSensorToMechanismRatio(6.75) is applied.)

A 4.44 × 6.75 × 0.319 = 9.55 m/s
B 4.44 × 0.319 = 1.42 m/s — the gear ratio is already applied by withSensorToMechanismRatio(), so the velocity signal reports wheel RPS directly.
C 4.44 ÷ 0.319 = 13.9 m/s
D 4.44 ÷ 6.75 × 0.319 = 0.21 m/s

3. The steering motor has kP = 50.0 in Slot 0 and uses the remote CANcoder as feedback. The current angle is 0.10 rotations and the target is 0.35 rotations. Approximately what output voltage does the P controller apply? (Assume kD = 0, kI = 0, and the controller output is in volts where 12V is full.)

A 50 × (0.35 − 0.10) = 12.5V — saturated at motor max
B 50 × (0.35 − 0.10) = 12.5V, which saturates at the 12V battery voltage and produces a maximum drive toward the target. The motor accelerates hard. As it approaches the target and error shrinks, output voltage drops proportionally — this is why kD helps prevent overshoot.
C 50 × 0.35 = 17.5V — the target angle times kP
D 0.25V — the output can never saturate with a P-only controller

💪 Practice Prompt

Build and Test the Complete SwerveModule

Create SwerveModule.java with the constructor, hardware fields, control request fields, and all three configuration methods from this lesson. Use your Constants values from Lesson 2. Add a String m_name field passed into the constructor for telemetry prefixing (e.g., "FL", "FR", "BL", "BR").
Implement setDesiredState() exactly as shown: refresh the steer angle signal, call optimize(), apply the near-zero guard, convert m/s to wheel RPS for the drive command, and convert Rotation2d to rotations for the steer command. Double-check that you're dividing by kWheelCircumference (not multiplying) for the drive RPS conversion.
Implement getState() and getPosition(). Add updateTelemetry(m_name) that publishes SteerAngleDeg, DriveVelocityMPS, and DriveCurrentA. In the drivetrain's periodic(), call updateTelemetry() on each module.
With the robot on blocks, instantiate a single SwerveModule in RobotContainer using the Front Left CAN IDs and offset. In teleopInit(), call setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(0))). Enable and confirm the wheel holds at 0°. Then test 90°, 180°, −90°. Verify the steering angle on SmartDashboard matches each commanded angle within ±3°.
Stretch goal: Enable ClosedLoopGeneral.withContinuousWrap(true) in the steer motor's TalonFXConfiguration. Command 170°, then immediately command −170°. Without continuous wrap, the module rotates 340°. With it, the module rotates 20°. Verify the difference on SmartDashboard. This single config line is the difference between sluggish and snappy steering at angle boundaries.

The SwerveModule Class

The SwerveModule Class

By the end of this lesson, you will:

The SwerveModule Class: One Job, Done Completely

Two Control Problems, Two Strategies

Steering: Position PID

Drive: Velocity Feedforward + P

Steering PID Behavior Visualizer

Part 1: The Constructor and Hardware Configuration

Part 2: setDesiredState() — The Core Method

Unit Conversions: The Full Chain

First-Bringup Telemetry: What to Watch

Knowledge Check

Build and Test the Complete SwerveModule

Part 2: `setDesiredState()` — The Core Method