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

Setting up swerve drive (C++)

Goal: Create a fully functional SwerveDrive subsystem in C++ with field-relative driving, odometry, and simulation.

The gains and encoder offsets in this guide are placeholders, as the overview explains. The code blocks are pieces of one DriveSubsystem class: the hardware, configs, wrappers and modules are all members of it. The complete class is the SwerveSubsystem.cpp example at the end of the page.


Prerequisites

Install the Commands v2 YAMS vendordep (https://cdn.yassrobotics.com/yams_commands2.json). YAMS.hpp does not pull in the vendor wrappers, so include the wrapper for your motor controller explicitly.

#include <yams/YAMS.hpp>

// Or individually:
#include <yams/mechanisms/swerve/SwerveDrive.hpp>
#include <yams/mechanisms/swerve/SwerveDriveConfig.hpp>
#include <yams/mechanisms/swerve/SwerveModule.hpp>
#include <yams/mechanisms/swerve/utility/SwerveInputStream.hpp>
#include <yams/mechanisms/config/SwerveModuleConfig.hpp>
#include <yams/gearing/MechanismGearing.hpp>
#include <yams/motorcontrollers/SmartMotorControllerConfig.hpp>

// Vendor wrapper (always included individually):
#include <yams/motorcontrollers/remote/TalonFXWrapper.hpp>

// WPILib 2027 headers used below:
#include <wpi/commands2/CommandPtr.hpp>
#include <wpi/commands2/SubsystemBase.hpp>
#include <wpi/commands2/button/CommandXboxController.hpp>
#include <wpi/math/controller/PIDController.hpp>
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
#include <wpi/math/geometry/Pose2d.hpp>
#include <wpi/math/geometry/Translation2d.hpp>
#include <wpi/math/system/DCMotor.hpp>

WPILib 2027 renamed ChassisSpeeds to wpi::math::ChassisVelocities and SwerveModuleState to wpi::math::SwerveModuleVelocity. The YAMS method names (SetFieldRelativeChassisSpeeds, ...) are unchanged.


1

Configure drive and azimuth motor controllers

Each swerve module needs two SmartMotorController instances: one for the drive wheel (velocity control) and one for the azimuth (steering angle, position control).

Repeat for each module (FR, BL, BR), adjusting the telemetry name and CAN IDs.

Check it: the project builds.

2

Construct motor controllers

The wrapper takes pointers to the vendor hardware object and to the config. Both must outlive the wrapper, so keep them as subsystem members.

Check it: the project builds.

3

Create a SwerveModuleConfig for each module

Pass drive and azimuth motor controllers, then set the wheel radius, the module location (relative to robot center), the absolute encoder supplier, and its offset.

Repeat for FrontRight, BackLeft, BackRight with their positions and offsets:

Module
Location

Front Left

{ +0.381_m, +0.381_m }

Front Right

{ +0.381_m, -0.381_m }

Back Left

{ -0.381_m, +0.381_m }

Back Right

{ -0.381_m, -0.381_m }

+x is toward the front of the robot and +y toward the left.

Check it: the project builds.

4

Construct SwerveModule instances

Check it: the project builds.

5

Create a SwerveDriveConfig

Collect the four modules, set kinematic limits, and provide a gyro supplier. WithGyro takes either the yaw as a wpi::units::degree_t, as here, or the gyro's full attitude as a wpi::math::Rotation3d.

Check it: the project builds.

6

Construct SwerveDrive

The template parameter must match the number of modules passed to WithModules:

Check it: run the simulation. The code starts without an exception.

7

Integrate into a subsystem

The subsystem owns the SwerveInputStream so it lives as long as the drive command that reads it. The constructor and the members are left out here: they are the pieces above.

Check it: run the simulation. The drive shows in NetworkTables under Mechanisms -> swerve, with a field showing the robot at the origin.

8

Set up driver input with SwerveInputStream

SwerveInputStream<4> translates raw joystick axes into field-relative wpi::math::ChassisVelocities. Pass the SwerveDrive reference as the first argument, then set a rotation axis with WithControllerRotationAxis.

WithTelemetry(name, verbosity) publishes the stream under SwerveInputStream/<name> every time it is read. At HIGH the stream is also live tunable from the dashboard. Call it last, after the rest of the configuration.

Check it: in simulation, enable teleop with a controller connected (or a keyboard joystick in the Sim GUI). The left stick drives the robot on the field and the right stick turns it.

9

Heading-snap mode (optional)

Clone the input stream to add heading control without affecting the base stream. A Clone() has no telemetry, so give it its own name if you want it published.

Check it: drive with headingInput and hold the right stick in. The robot turns to face where the right stick points, and holds that heading.

10

Azimuth encoder seeding

Azimuth encoders are seeded automatically during SwerveModule construction. If you need to re-seed after the CAN bus has fully settled, call SeedAzimuthEncoder() on each module:

Check it: on the robot, with the robot disabled, turn each wheel by hand. Its azimuth angle in telemetry follows the wheel. If a wheel points the wrong way when enabled, its encoder offset is wrong.

11

Auto-align (DriveToPose) and live PID tuning

DriveToPose(wpi::math::Pose2d) returns a wpi::cmd::CommandPtr that drives the robot to a field-relative pose using the WithTranslationController/WithRotationController PID gains set on SwerveDriveConfig:

At TelemetryVerbosity::HIGH, SwerveDrive<N> also publishes a live tuning command for these gains and for the module gains, so you can tune them without redeploying. SwerveDrive (C++): Auto-Align lists its fields.

Check it: in simulation, schedule DriveToPoseCommand with a pose 2 m forward and 1 m left of the origin. The robot drives there and stops.


Key SwerveDrive methods

Method
Description

SetFieldRelativeChassisSpeeds(ChassisVelocities)

Field-relative drive

SetRobotRelativeChassisSpeeds(ChassisVelocities)

Robot-relative drive

Drive(std::function<ChassisVelocities()>)

Returns a run CommandPtr for continuous robot-relative driving

DriveToPose(Pose2d)

Returns a CommandPtr that PID-drives to a field-relative pose

DriveToPoseSetpoint(Pose2d)

Computes one loop's ChassisVelocities toward a pose (building block behind DriveToPose)

SetTranslationPID(PIDController) / SetRotationPID(PIDController)

Replace the auto-align PID controllers (integrator preserved unless gains changed)

ResetTranslationPID() / ResetRotationPID()

Reset the auto-align PID controllers' internal state

LockPose()

X-pattern to resist pushing

GetPose()

Current field-relative pose from odometry

GetGyroAngle()

Current gyro heading as a degree_t

GetGyroRotation3d()

The gyro's full attitude as a Rotation3d

ResetOdometry(Pose2d)

Reset odometry to a known pose

ZeroGyro()

Zero the gyro heading

AddVisionMeasurement(Pose2d, second_t)

Fuse vision pose into odometry

GetSimPose()

Ground-truth simulated pose, useful for feeding a simulated vision system

UpdateTelemetry()

Call in Periodic()

SimIterate()

Call in SimulationPeriodic()


Notes

SwerveInputStream::Of() takes translation axes only. Add rotation later with WithControllerRotationAxis() or switch to heading control with WithControllerHeadingAxis() + WithHeadingControl().

Gyro mounted on its side? Supply the full attitude with WithGyro (a Rotation3d), then call .WithGyroHeadingAxis(yams::mechanisms::config::GyroAxis::ROLL) or GyroAxis::PITCH on SwerveDriveConfig to say which gyro axis points up through the robot. The default is GyroAxis::YAW, for a gyro mounted flat.

If it did not work

  • It does not build. Check the vendordep, the wrapper #include, and that every hardware object, config, wrapper and module is a member that outlives what points at it.

  • The robot drives the wrong way, or spins when it should drive straight. Check each module's WithLocation signs, then that the drive motors are not inverted.

  • A wheel points the wrong way, or the wheels fight each other. The absolute encoder offsets are wrong. Measure them as in On the real robot.

  • Field-relative driving is rotated. The gyro supplier is wrong, or the gyro was not zeroed facing away from the driver.

Troubleshooting covers more.


Examples

Complete C++ implementations from the cpptest reference project:

https://github.com/Yet-Another-Software-Suite/YAMS/blob/master/examples/cpptest/src/main/cpp/subsystems/SwerveSubsystem.cpp

Last updated

Was this helpful?