Ctrlk
For the complete documentation index, see llms.txt. This page is also available as Markdown.

Swerve Drive

We will learn how to program a Swerve Drive!

Swerve drive requires more hardware configuration than a tank drive. Make sure you have installed REVLib (for SparkMax motors) and Phoenix 6 (for CANcoder and Pigeon2) before starting.

Intro

At the end of this tutorial you will have a full swerve drive subsystem that works in both real life and simulation with the same code!

Details

This swerve drive will be using the following hardware and control details:

  • 4x SparkMax (NEO) drive motors, CAN IDs 1, 4, 7, 10

  • 4x SparkMax (NEO) azimuth motors, CAN IDs 2, 5, 8, 11

  • 4x CANcoder absolute encoders, CAN IDs 3, 6, 9, 12

  • Pigeon2 gyro, CAN ID 14

  • SDS MK4i modules: drive gear ratio 12.75:1, steer gear ratio 6.75:1, 4-inch wheels

  • Left stick controls translation, right stick X controls rotation

Simulation Processing Requirements: YAMS simulates each motor individually. With 4 modules containing 2 motors each, that is 8 SmartMotorController instances running physics simulation simultaneously. This provides highly accurate simulation but requires more processing power than simplified kinematic-only simulations.

Let's make a Swerve Drive!

1

Configure the Drive Motor Controllers

We start with the SmartMotorControllerConfig for the drive motors. The drive motors use velocity control, so we configure a PID controller and a feedforward.

SwerveSubsystem.java
package frc.robot.subsystems;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import yams.gearing.MechanismGearing;
import yams.motorcontrollers.SmartMotorControllerConfig;
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;

public class SwerveSubsystem extends SubsystemBase {

  // kP=0.3 trims velocity error left after the feedforward.
  // kV = 12 V / (max wheel speed in rad/s): at full speed the feedforward
  // alone commands 12 V, leaving PID to correct only disturbances.
  private SmartMotorControllerConfig buildDriveCfg() {
    return new SmartMotorControllerConfig(this)
        .withWheelDiameter(Inches.of(4))
        .withClosedLoopController(0.3, 0, 0)
        .withGearing(new MechanismGearing(12.75))  // MK4i L1 ratio
        .withFeedforward(new SimpleMotorFeedforward(0, 12.0 / (MetersPerSecond.of(1).in(MetersPerSecond) / Inches.of(4).in(Meters)), 0.01))
        .withStatorCurrentLimit(Amps.of(40))        // prevents belt slip
        .withTelemetry("driveMotor", TelemetryVerbosity.HIGH);
  }

}

The withFeedforward calculates kV from your maximum linear velocity and wheel diameter so the feedforward alone handles most of the work.

2

Configure the Azimuth Motor Controllers

The azimuth (steering) motors use position control. A single kP=1 is typically enough at a 6.75:1 reduction because the gear ratio makes the output stiff.

SwerveSubsystem.java
package frc.robot.subsystems;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import yams.gearing.MechanismGearing;
import yams.motorcontrollers.SmartMotorControllerConfig;
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;

public class SwerveSubsystem extends SubsystemBase {

  private SmartMotorControllerConfig buildDriveCfg() {
    return new SmartMotorControllerConfig(this)
        .withWheelDiameter(Inches.of(4))
        .withClosedLoopController(0.3, 0, 0)
        .withGearing(new MechanismGearing(12.75))
        .withFeedforward(new SimpleMotorFeedforward(0, 12.0 / (MetersPerSecond.of(1).in(MetersPerSecond) / Inches.of(4).in(Meters)), 0.01))
        .withStatorCurrentLimit(Amps.of(40))
        .withTelemetry("driveMotor", TelemetryVerbosity.HIGH);
  }

  // kP=1 with a 6.75:1 reduction gives a stiff, responsive steer.
  // 20 A is enough, the steer motor almost never stalls during normal driving.
  private SmartMotorControllerConfig buildAzimuthCfg() {
    return new SmartMotorControllerConfig(this)
        .withClosedLoopController(1, 0, 0)
        .withFeedforward(new SimpleMotorFeedforward(0, 1))
        .withGearing(new MechanismGearing(6.75))   // MK4i steer ratio
        .withStatorCurrentLimit(Amps.of(20))
        .withTelemetry("angleMotor", TelemetryVerbosity.HIGH);
  }


}
3

Build a SwerveModule Factory

Now we write a helper method that takes the two SparkMax objects and a CANcoder, wraps them in SmartMotorControllers, and returns a configured SwerveModule.

SwerveSubsystem.java
package frc.robot.subsystems;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;

import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import yams.gearing.MechanismGearing;
import yams.mechanisms.config.SwerveModuleConfig;
import yams.mechanisms.swerve.SwerveModule;
import yams.motorcontrollers.SmartMotorController;
import yams.motorcontrollers.SmartMotorControllerConfig;
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
import yams.motorcontrollers.local.SparkWrapper;


public class SwerveSubsystem extends SubsystemBase {

  private SmartMotorControllerConfig buildDriveCfg() { /* ... same as before ... */ }
  private SmartMotorControllerConfig buildAzimuthCfg() { /* ... same as before ... */ }

  /**
   * All four modules share the same gearing and PID gains, so one factory
   * method creates all of them. Pass each module a unique name and location.
   */
  public SwerveModule createModule(SparkMax drive, SparkMax azimuth,
                                   CANcoder absoluteEncoder, String moduleName,
                                   Translation2d location) {
    SmartMotorController driveSMC   = new SparkWrapper(drive,   DCMotor.getNEO(1), buildDriveCfg());
    SmartMotorController azimuthSMC = new SparkWrapper(azimuth, DCMotor.getNEO(1), buildAzimuthCfg());

    return new SwerveModule(new SwerveModuleConfig(driveSMC, azimuthSMC)
        // CANcoder eliminates the need to home the steer motor at startup.
        .withAbsoluteEncoder(absoluteEncoder.getAbsolutePosition().asSupplier())
        .withTelemetry(moduleName, TelemetryVerbosity.HIGH)
        .withLocation(location)
        // State optimization rotates the module at most 90 deg instead of 180 deg + reversing drive.
        .withOptimization(true));
  }


}

The withLocation(Translation2d) call uses WPILib's coordinate convention: +X is toward the front of the robot, +Y is toward the left side.

4

Instantiate the Four Modules and Create SwerveDrive

In the constructor, we create all four modules and pass them to SwerveDriveConfig. The gyro supplier gives field-relative driving its heading reference.

SwerveSubsystem.java
package frc.robot.subsystems;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;

import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import yams.gearing.MechanismGearing;
import yams.mechanisms.config.SwerveModuleConfig;
import yams.mechanisms.config.SwerveDriveConfig;
import yams.mechanisms.swerve.SwerveDrive;
import yams.mechanisms.swerve.SwerveModule;
import yams.motorcontrollers.SmartMotorController;
import yams.motorcontrollers.SmartMotorControllerConfig;
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
import yams.motorcontrollers.local.SparkWrapper;

public class SwerveSubsystem extends SubsystemBase {

  private final SwerveDrive drive;
  private final Field2d field = new Field2d();

  // ... buildDriveCfg(), buildAzimuthCfg(), createModule() from previous steps ...

  public SwerveSubsystem() {
    // CAN ID 14, update to match your robot's Pigeon2 CAN ID.
    Pigeon2 gyro = new Pigeon2(14);

    // Module locations: +X forward, +Y left. 24-inch offsets assume module
    // centers are 24 in from the robot center, update to match your chassis.
    // CAN IDs are grouped as (drive, steer, CANcoder) per module.
    var fl = createModule(new SparkMax(1, MotorType.kBrushless),
                          new SparkMax(2, MotorType.kBrushless),
                          new CANcoder(3), "frontleft",
                          new Translation2d(Inches.of(24), Inches.of(24)));
    var fr = createModule(new SparkMax(4, MotorType.kBrushless),
                          new SparkMax(5, MotorType.kBrushless),
                          new CANcoder(6), "frontright",
                          new Translation2d(Inches.of(24), Inches.of(-24)));
    var bl = createModule(new SparkMax(7, MotorType.kBrushless),
                          new SparkMax(8, MotorType.kBrushless),
                          new CANcoder(9), "backleft",
                          new Translation2d(Inches.of(-24), Inches.of(24)));
    var br = createModule(new SparkMax(10, MotorType.kBrushless),
                          new SparkMax(11, MotorType.kBrushless),
                          new CANcoder(12), "backright",
                          new Translation2d(Inches.of(-24), Inches.of(-24)));

    SwerveDriveConfig config = new SwerveDriveConfig(this, fl, fr, bl, br)
        // gyro.getYaw() gives the heading used for field-relative driving.
        .withGyro(gyro.getYaw().asSupplier())
        .withStartingPose(new Pose2d(0, 0, Rotation2d.fromDegrees(0)))
        // Translation and rotation PIDs are used by driveToPose(); kP=1 is a conservative start.
        .withTranslationController(new PIDController(1, 0, 0))
        .withRotationController(new PIDController(1, 0, 0));

    drive = new SwerveDrive(config);
    SmartDashboard.putData("Field", field);
  }

}
5

Add Periodic Updates and Drive Commands

periodic() must call drive.updateTelemetry() to keep odometry and SmartDashboard fresh. simulationPeriodic() must call drive.simIterate() to step the physics engine.

SwerveSubsystem.java
  // ... fields and constructor from previous steps ...

  /**
   * Drive the robot with field-relative chassis speeds.
   */
  public Command drive(Supplier<ChassisSpeeds> speedsSupplier) {
    return run(() -> drive.setFieldRelativeChassisSpeeds(speedsSupplier.get()))
        .withName("Field Oriented Drive");
  }

  /** Drive to a specific field pose. */
  public Command driveToPose(Pose2d pose) { return drive.driveToPose(pose); }

  /** Lock wheels in X pattern to resist pushing. */
  public Command lock() { return run(drive::lockPose); }


  @Override
  public void periodic() {
    drive.updateTelemetry();           // updates pose estimator and telemetry
    field.setRobotPose(drive.getPose()); // keeps the Field2d widget current
  }

  @Override
  public void simulationPeriodic() {
    drive.simIterate();  // steps the motor physics simulation
  }

You can also expose getPose(), resetOdometry(), and addVisionMeasurement() as needed for autonomous and vision integration.

SwerveDrive already builds and publishes its own Field2d internally (to Mechanisms/<name>/field), so the Field2d field instance above is actually redundant. Call drive.getField2d() instead of keeping a separate widget; this also lets vision or autonomous code plot extra objects (e.g. detected AprilTags, path previews) onto the same widget the drive already publishes, rather than creating and registering yet another one.

In simulation, drive.getSimPose() returns a ground-truth Pose2d computed by assuming every module hit its last-commanded SwerveModuleState perfectly, separate from getPose()'s noisy, fused odometry estimate. It's a convenient "known truth" source for a simulated vision subsystem to generate synthetic target detections from, letting you test vision-dependent code without physical hardware. It only advances during simIterate() and stays at the configured starting pose on a real robot.

Tuning the driveToPose() PID gains live: with TelemetryVerbosity.HIGH (the default), YAMS publishes a "Live Tuning"-style command to SmartDashboard at Mechanisms/<name>/tuning/driveToPose, along with a tunable target pose (autoalign/setpoint/x/autoalign/setpoint/y in meters, autoalign/setpoint/rot in degrees) gated by an autoalign/enabled boolean, plus tunable translation/rotation P/I/D gains, all under the Tuning NetworkTables table. Schedule that command from your dashboard, set autoalign/setpoint/x/autoalign/setpoint/y/autoalign/setpoint/rot to the pose you want and flip autoalign/enabled to true, and it repeatedly drives toward that pose using whatever gains you edit live in NetworkTables, so you can dial in withTranslationController(...)/withRotationController(...) without redeploying code. setRotationPID(...)/setTranslationPID(...) only reset a PID controller when a gain actually changes, so tuning doesn't wipe out the integrator every loop.

6

Create a SwerveInputStream and Bind the Default Command

SwerveInputStream converts raw joystick values into ChassisSpeeds with deadband, cubing, and alliance-relative field orientation applied automatically. Set it as the subsystem's default command in RobotContainer.

SwerveSubsystem.java
  // Add this method to SwerveSubsystem
  public SwerveInputStream getChassisSpeedsSupplier(DoubleSupplier translationX,
                                                     DoubleSupplier translationY,
                                                     DoubleSupplier rotation) {
    return new SwerveInputStream(drive, translationX, translationY, rotation)
        .withMaximumAngularVelocity(DegreesPerSecond.of(360))
        .withMaximumLinearVelocity(MetersPerSecond.of(1))
        .withDeadband(0.01)
        .withCubeRotationControllerAxis()     // non-linear rotation response
        .withCubeTranslationControllerAxis()  // non-linear translation response
        .withAllianceRelativeControl();        // alliance-aware field-relative
  }
RobotContainer.java
public class RobotContainer {

  private final SwerveSubsystem drive = new SwerveSubsystem();
  private final CommandXboxController xboxController = new CommandXboxController(0);

  public RobotContainer() {
    DriverStation.silenceJoystickConnectionWarning(true);

    // Left stick translates, right stick X rotates.
    drive.setDefaultCommand(drive.drive(
        drive.getChassisSpeedsSupplier(
            xboxController::getLeftY,
            xboxController::getLeftX,
            xboxController::getRightX)));

    configureBindings();
  }

  private void configureBindings() {
    xboxController.button(5).whileTrue(drive.driveToPose(
        new Pose2d(Meters.of(3), Meters.of(3), Rotation2d.fromDegrees(30))));
  }
}

Complete Example

The following files show a complete swerve drive implementation matching the hardware described in this tutorial.

https://github.com/Yet-Another-Software-Suite/YAMS/blob/master/examples/swerve_drive/java/frc/robot/subsystems/SwerveSubsystem.java
https://github.com/Yet-Another-Software-Suite/YAMS/blob/master/examples/swerve_drive/java/frc/robot/RobotContainer.java

Next Steps

PreviousDouble FlyWheel
NextHow do I use a vendor hardware config?

Last updated

Was this helpful?