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.
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.
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:
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.
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.
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.
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.
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.
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
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
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
WithLocationsigns, 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:
Related pages
Last updated
Was this helpful?
