21. Swerve Drive#

Why it matters#

The West Coast Drive can go forward, backward, and turn. To move sideways it has to turn, drive, and turn back. A defender who knows that can pin it against a wall.

A swerve drive has four wheels that each steer on their own. Point all four wheels sideways and the robot slides sideways. Point them in a circle and it spins in place. Do both at once and it can drive across the field in a straight line while spinning to face a goal. Most competitive FRC robots now drive swerve.

You won't write swerve from scratch: teams start from a vendor template. This lesson explains what the template is doing, using everything from Lessons 12–19, so you can read it, tune it, and fix it.

Read this#

• Swerve Drive Kinematics (WPILib)
• Swerve Drive Odometry (WPILib)
• Your module's template: CTRE Tuner X Swerve Generator (Kraken/Falcon + CANcoder), REV MAXSwerve template (NEO/Vortex), or YAGSL (many combinations)

Walkthrough#

What's in a swerve module#

Each corner of the robot is a module with:

So a swerve drive is just four velocity controllers and four position controllers, working together. Every idea in it is one you've already used.

Holonomic driving#

A tank drive takes two numbers: left and right. A swerve drive takes three, as a ChassisSpeeds (from Lesson 19):

A drivetrain that can do all three independently is called holonomic.

Kinematics: robot speed → four wheel states#

SwerveDriveKinematics needs to know where each module is, measured from the center of the robot (+X forward, +Y left):

  // Module positions from the center of the robot, in meters. Order: FL, FR, BL, BR.
  private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
      new Translation2d(0.28, 0.28),     // front left
      new Translation2d(0.28, -0.28),    // front right
      new Translation2d(-0.28, 0.28),    // back left
      new Translation2d(-0.28, -0.28));  // back right

Give it a ChassisSpeeds and it returns four SwerveModuleStates: a speed and an angle for each wheel.

  /** Drives the robot. x and y in m/s, turn in rad/s. */
  public void drive(double xSpeed, double ySpeed, double turnSpeed, boolean fieldRelative) {
    ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, turnSpeed);
    if (fieldRelative) {
      // Joystick "forward" means "away from the driver", whichever way the robot faces
      speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
    }
    // Correct for the robot turning during the 20 ms step, so it doesn't drift sideways while spinning
    speeds = ChassisSpeeds.discretize(speeds, 0.02);

    SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(speeds);
    // If any wheel would have to go faster than it can, slow all four down by the same ratio
    SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeedMetersPerSecond);

    for (int i = 0; i < 4; i++) {
      m_modules[i].setDesiredState(states[i]);
    }
  }

The order FL, FR, BL, BR must be the same everywhere: in the kinematics, the module array, and the odometry. Swap two and the robot drives confidently in the wrong direction.

Field-relative driving#

Robot-relative: pushing the stick forward drives toward the robot's front. Hard to drive once the robot has turned around.

Field-relative: pushing the stick forward drives away from the driver, no matter which way the robot faces. fromFieldRelativeSpeeds uses the gyro heading to rotate the joystick's direction into the robot's frame. Almost every team drives field-relative.

Inside one module#

Each module turns its desired state into two motor commands:

  /** Point the wheel and spin it at the requested state. */
  public void setDesiredState(SwerveModuleState desired) {
    Rotation2d current = getSteerAngle();

    // Never turn the wheel more than 90°: spinning the wheel backward is the same as turning it 180°
    desired.optimize(current);
    // Slow the wheel down while it's still turning toward the right direction
    desired.cosineScale(current);

    // Drive motor: velocity control (feedforward + PID), like Lesson 19's tank drive
    double driveVolts = m_driveFeedforward.calculate(desired.speedMetersPerSecond)
        + m_drivePID.calculate(getDriveVelocity(), desired.speedMetersPerSecond);
    // Steer motor: position control. Continuous input makes -179° and +179° neighbors.
    double steerVolts = m_steerPID.calculate(current.getRadians(), desired.angle.getRadians());

    m_driveMotor.setVoltage(driveVolts);
    m_steerMotor.setVoltage(steerVolts);
  }

Real templates usually run both loops on the motor controller (Phoenix 6 or REVLib closed-loop control) instead of on the roboRIO, because it's faster. The ideas are the same.

Absolute encoder offsets#

Every steer encoder reads some angle when the wheel points straight ahead, and it's different for every module. That reading is the module's offset. Get it wrong and that wheel points the wrong way, and the robot fights itself.

Templates have a procedure for this: point all four wheels straight ahead (bevel gears all facing the same side), then read and save the offsets. CTRE's Tuner X generator does it for you. Redo it any time a module is taken apart or swapped.

Odometry and the rest#

Swerve odometry works like the WCD's (Lesson 17), with one difference: each module reports how far it rolled and which way it pointed, as a SwerveModulePosition. SwerveDrivePoseEstimator replaces DifferentialDrivePoseEstimator for vision (Lesson 18). For PathPlanner (Lesson 19), set Holonomic mode in the app and use PPHolonomicDriveController instead of PPLTVController. Holonomic paths can turn the robot independently of the direction it's driving.

In AdvantageScope's Swerve tab (Lesson 20), log the desired and measured SwerveModuleState[] and you'll see four arrows: what each module was told to do and what it actually did.

Getting started on a real swerve#

1. Pick the template for your modules (see "Read this").
2. Enter the hardware: CAN IDs, gear ratios, wheel diameter, module positions.
3. Set the encoder offsets with the wheels pointing straight.
4. Check with the robot on blocks: push forward on the stick, and all four wheels should point forward and spin forward. Then left, then rotate. Fix one thing at a time.
5. Tune the steer PID first (snappy, no wobble), then the drive feedforward (SysId) and PID.
6. Measure the real max speed and put it in the template and in PathPlanner's robot config.

Try it#

• On paper: the robot drives forward at 1 m/s with no turning. What angle and speed does each module get?
• On paper: the robot spins in place counterclockwise. Which way does each wheel point?
• Open a swerve template for your team's modules and find the kinematics, optimize, and the encoder offsets
• Run a template in simulation and drive it field-relative with an Xbox controller

Check yourself#

Parts of this lesson are adapted from the Swerve Drive Example and Odometry lessons of Mantik by Itkan Robotics (CC BY 4.0).