Swerve is a type of drivetrain where all wheels can rotate and move independently of one another. It allows moving the robot in any direction with any angular velocity while maintaining power and speed. This won't go in depth into the math and algorithms behind swerve drives, only how to implement them in code. To learn more about the dynamics, see this blog post.

First, you need to characterize the swerve drive. This means measuring the dimensions, gear ratios, etc., and setting PID constants in the Constants file. There are a lot of constants, so seeing them all at once can be overwhelming. For that reason, we will break it down into more manageable chunks.

Measure the distance between the centers of swerve modules, as well as the wheel radius, and then make constants for them as such:

private static final Measure<Distance> swerveWidth = Inches.of(24); // width between centers of swerve modules
                                                                            // from left to right
private static final Measure<Distance> swerveLength = Inches.of(24); // length between centers of swerve modules
                                                                             // from front to back

private static final Measure<Distance> wheelRadius = Inches.of(1.97);

Next, enter the gear ratios you are using (if you are unsure, check the page you ordered from, it should have them).

private static final double driveMotorGearRatio = (8.14 / 1.0); // 8.14:1
private static final double steerMotorGearRatio = (150.0 / 7 / 1.0); // 150/7:1

Also, while it is not strictly necessary, the coupling gear ratio can help the swerve drive be as accurate as possible. This is the ratio between the number of rotations of the drive motor to the number of azimuthal (steering) rotations of the wheel, typically the inverse of the first-stage gear ratio.

private static final double couplingGearRatio = 50.0 / 14.0;

Slip current is the current at which the wheel will slip on the floor. We typically just use a value of ~400 A, because it can be hard to precisely measure.

private static final Measure<Current> slipCurrent = Amps.of(400);

These characterize the behavior of the motors in moving the wheels correctly. They require quite a bit of tuning. Generally, for the steering, set the kV (output per unit velocity) by measuring it for best results, then increase kP until the wheels oscilate, then increase kD to stop the oscilations and kI to remove any residual error. Also, set the kS (output to overcome static friction) through testing as well. The drive gains require less tuning, with mainly just the kV and a relatively low kP.

Here are the gains for our Crescendo bot:

private static final Slot0Configs steerMotorGains = new Slot0Configs()
        .withKP(200.0) // output per unit error in position (rotations)
        .withKI(50.0) // output per unit integrated error (rotations*s)
        .withKD(50.0) // output per unit of error derivative (rps)
        .withKS(10.0) // output to overcome static friction
        .withKV(2.5); // output per unit of velocity (rps)
private static final Slot0Configs driveMotorGains = new Slot0Configs()
        .withKP(1.0) // output per unit error in position (rps)
        .withKI(0.0) // output per unit integrated error (rotations)
        .withKD(0.0) // output per unit of error derivative (rps/s)
        .withKS(0) // output to overcome static friction
        .withKV(0.123) // output per unit of velocity (rps)
        .withKA(0); // output per unit of acceleration (rps/s)

Output type how the motors are controlled. We use TorqueCurrentFOC, which controllers the current to the motors:

private static final ClosedLoopOutputType steerMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC;
private static final ClosedLoopOutputType driveMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC;

RequestType determines how the motors' targets should be set and reached:

public static final DriveRequestType driveRequestType = DriveRequestType.Velocity;
public static final SteerRequestType steerRequestType = SteerRequestType.MotionMagicExpo;

We use Velocity for the drive request, which will try to move the drive motors at a set velocity and maintain a constant drive speed. We use MotionMagicExpo for the steer request because it aims for a set position with CTRE's custom motion profile that allows fast and accurate control.

private static final Measure<Velocity<Distance>> speedAt12Volts = FeetPerSecond.of(13.0);
private static final SteerFeedbackType feedbackSource = SteerFeedbackType.FusedCANcoder;
...
public static final Measure<Velocity<Distance>> velocityDeadband = defaultDriveSpeed.times(0.02);
public static final Measure<Velocity<Angle>> rotationDeadband = defaultRotSpeed.times(0.01);

speedAt12Volts, as the name implies, defines how fast the robot moves at full speed. This can be hard to measure, but it is only used in open-loop control, which we rarely use for swerve, so an estimate works well enough.

feedbackSource defines where the steer motors should get their sensor info from. FusedCANcoder is the typical value which just means the motor will use the connected CANcoder in conjunction with its internal rotor.

The deadbands set the minimum speeds for the drivetrain to try and move. Keep these low.

Now, it is time to combine all of these constants into one constants factory. You will later use this to create a SwerveDrivetrain.

private static final SwerveModuleConstantsFactory constantsCreator = new SwerveModuleConstantsFactory()
                .withDriveMotorGearRatio(driveMotorGearRatio)
                .withSteerMotorGearRatio(steerMotorGearRatio)
                .withWheelRadius(wheelRadius.in(Inches))
                .withSlipCurrent(slipCurrent.in(Amps))
                .withSteerMotorGains(steerMotorGains)
                .withDriveMotorGains(driveMotorGains)
                .withSteerMotorClosedLoopOutput(steerMotorClosedLoopOutput)
                .withDriveMotorClosedLoopOutput(driveMotorClosedLoopOutput)
                .withSpeedAt12VoltsMps(speedAt12Volts.in(MetersPerSecond))
                .withFeedbackSource(feedbackSource)
                .withCouplingGearRatio(couplingGearRatio);

Now, we need to characterize each module. Each module gets its own class:

private static final class FrontLeft {
    private static final int steerId = 7; // CAN id of steer motor
    private static final int driveId = 22; // CAN id of drive motor
    private static final int encoderId = 2; // CAN id of encoder (CANcoder)
    private static final double encoderOffset = -0.446;
    private static final double xPos = swerveLength.in(Meters) / 2; // meters to front
    private static final double yPos = swerveWidth.in(Meters) / 2; // meters to left
    private static final boolean invertedSteer = true;
    private static final boolean invertedDrive = true;

    public static final SwerveModuleConstants moduleConstants = constantsCreator.createModuleConstants(
            steerId, driveId, encoderId, encoderOffset, xPos, yPos, invertedDrive)
            .withSteerMotorInverted(invertedSteer);
}

The one tricky part of this is the encoderOffset. If you have no starting value, set it to zero. Then, once everything is set up, physically align all the wheels, then open Tuner X to read off the encoder values that you can then negate and set the offsets to.

You repeat this whole process for each swerve module.

It's finally time to create the drivetrain! First, we create SwerveDrivetrainConstants from previously-defined constants:

private static final SwerveDrivetrainConstants drivetrainConstants = new SwerveDrivetrainConstants()
        .withCANbusName(HighSpeedCANbusName).withPigeon2Id(pigeon2Id);

And then the drivetrain:

public static final SwerveDrivetrain drivetrain = new SwerveDrivetrain(
        drivetrainConstants,
        FrontLeft.moduleConstants,
        FrontRight.moduleConstants,
        BackLeft.moduleConstants,
        BackRight.moduleConstants);

Current limits make sure the motors are not overloaded. The motors can draw up to the supply current threshold for the supply time threshold and continuously up to the supply current limit.

public static final CurrentLimitsConfigs angleCurrentLimits = new CurrentLimitsConfigs()
        .withSupplyCurrentLimit(20)
        .withSupplyCurrentThreshold(40)
        .withSupplyTimeThreshold(0.1);

public static final CurrentLimitsConfigs driveCurrentLimits = new CurrentLimitsConfigs()
        .withSupplyCurrentLimit(40)
        .withSupplyCurrentThreshold(40)
        .withSupplyTimeThreshold(0.1);

For the final constants, we have neutral modes. A NeutralModeValue of Brake means the motor will supply power to keep the motor static when set to neutral, while Coast allows the motor to continue to move freely.

public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Brake;
public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Coast;

The swerve subsystem contains most of the swerve drive functionality. For reference, see our 2024 Swerve.java file.

• SwerveDrivetrain drivetrain: the drivetrain defined in SwerveConstants
• Requests
  • SwerveRequest.FieldCentric fieldCentricRequest: used for most driving; relative to field
  • SwerveRequest.RobotCentric robotCentricRequest: drive the robot relative to its own orientation
  • SwerveRequest.ApplyChassisSpeeds chassisSpeedsRequest: used for auto control through a ChassisSpeeds object
• NetworkTables subscribers and publishers: defined in LoggingConstants for logging
• Field2d field: displays the robot's position on the game field for use in SmartDashboard/Shuffleboard

• Initializes instance variables
• Configs
  • Applies Pigeon configuration to the drivetrain
  • Loops through swerve modules and applies current limits
• Sets up heading PID controller
• Creates AprilTag NetworkTables listener to update odometry with any new vision measurements
• Puts the field onto SmartDashboard/Shuffleboard for drivers to see
• Applies vision measurement matrix to tune the influence of vision measurements on odometry

• driveFieldCentric: drive with the fieldCentricRequest
• driveFacingAngle: drive with the fieldCentricRequest while using the headingPID to face a desired angle
• driveFacingNote: same as driveFacingAngle, but uses different noteAlignmentPID
• driveRobotCentric with xVel, yVel, and rot: drive with the robotCentricRequest (not generally used)
• driveRobotCentric with chassisSpeeds: drive with the chassisSpeedsRequest
• getChassisSpeeds: returns current desired ChassisSpeeds
• resetPose: without an argument, resets the field-relative heading so that 'forward' is now the robot's current orientation. With an argument, resets to the supplied pose
• getPose: returns the robot's current Pose2d representing position and rotation
• applyVisionMeasurement: updates the swerve's odometry with a vision measurement; mostly called through the AprilTag listener
• periodic: called every update loop; updates field and logs ModuleStates, ModuleTargets, and rotation