> For the complete documentation index, see [llms.txt](https://yams.yassrobotics.com/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://yams.yassrobotics.com/api-reference/guides/setting-up-swerve-cpp.md).

# Setting Up Swerve Drive (C++)

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

***

## Prerequisites

```cpp
#include <yams/YAMS.hpp>

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

***

{% stepper %}
{% step %}

#### 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).

```cpp
// Drive motor config: velocity PID + feedforward
yams::motorcontrollers::SmartMotorControllerConfig driveConfig;
driveConfig
    .WithIdleMode(yams::motorcontrollers::SmartMotorControllerConfig::MotorMode::BRAKE)
    .WithMotorGearing(yams::gearing::MechanismGearing(6.75))
    .WithFeedback(0.1, 0.0, 0.0)
    .WithFeedforward(frc::SimpleMotorFeedforward<units::turns>{
        0.12_V, units::unit_t<frc::SimpleMotorFeedforward<units::turns>::kv_unit>{2.2},
        units::unit_t<frc::SimpleMotorFeedforward<units::turns>::ka_unit>{0.3}})   // kS, kV, kA
    .WithStatorCurrentLimit(60_A)
    .WithSubsystem(this)
    .WithTelemetry("DriveFL");

// Azimuth motor config: position PID, continuous wrapping
yams::motorcontrollers::SmartMotorControllerConfig azimuthConfig;
azimuthConfig
    .WithIdleMode(yams::motorcontrollers::SmartMotorControllerConfig::MotorMode::BRAKE)
    .WithMotorGearing(yams::gearing::MechanismGearing(12.8))
    .WithFeedback(7.0, 0.0, 0.3)
    .WithExternalEncoderDiscontinuityPoint(0.5_tr)
    .WithStatorCurrentLimit(40_A)
    .WithSubsystem(this)
    .WithTelemetry("AzimuthFL");
```

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

{% step %}

#### Construct motor controllers

```cpp
yams::motorcontrollers::remote::TalonFXWrapper driveMotorFL{
    new ctre::phoenix6::hardware::TalonFX(10),
    frc::DCMotor::KrakenX60(1),
    &driveConfig
};
yams::motorcontrollers::remote::TalonFXWrapper azimuthMotorFL{
    new ctre::phoenix6::hardware::TalonFX(11),
    frc::DCMotor::KrakenX60(1),
    &azimuthConfig
};
```

{% endstep %}

{% step %}

#### Create a `SwerveModuleConfig` for each module

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

```cpp
yams::mechanisms::config::SwerveModuleConfig frontLeftModuleConfig{
    &driveMotorFL, &azimuthMotorFL
};
frontLeftModuleConfig
    .WithWheelRadius(0.0508_m)                       // 2-inch wheel radius
    .WithLocation(frc::Translation2d{0.381_m, 0.381_m})  // front-left position
    .WithAbsoluteEncoderOffset(130.0_deg)
    .WithTelemetry("FrontLeft",
        yams::motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity::HIGH);
```

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 }` |
| {% endstep %} |                          |

{% step %}

#### Construct `SwerveModule` instances

```cpp
yams::mechanisms::swerve::SwerveModule frontLeft{&frontLeftModuleConfig};
yams::mechanisms::swerve::SwerveModule frontRight{&frontRightModuleConfig};
yams::mechanisms::swerve::SwerveModule backLeft{&backLeftModuleConfig};
yams::mechanisms::swerve::SwerveModule backRight{&backRightModuleConfig};
```

{% endstep %}

{% step %}

#### Create a `SwerveDriveConfig`

Collect the four modules, set kinematic limits, and provide a gyro supplier.

```cpp
yams::mechanisms::swerve::SwerveDriveConfig driveConfig;
driveConfig
    .WithSubsystem(this)
    .WithModules({&frontLeft, &frontRight, &backLeft, &backRight})
    .WithGyro([this]() { return imu_.GetYaw().GetValueAsDouble() * 1_deg; })
    .WithMaximumChassisSpeed(4.5_mps, units::degrees_per_second_t{360.0})
    .WithTranslationController(frc::PIDController{1.0, 0, 0})
    .WithRotationController(frc::PIDController{1.0, 0, 0})
    .WithTelemetry(
        "swerve",
        yams::motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity::HIGH);
```

{% endstep %}

{% step %}

#### Construct `SwerveDrive`

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

```cpp
yams::mechanisms::swerve::SwerveDrive<4> swerveDrive{&driveConfig};
```

{% endstep %}

{% step %}

#### Integrate into a subsystem

```cpp
class DriveSubsystem : public frc2::SubsystemBase {
public:
    DriveSubsystem() { /* configure as shown above */ }

    /** Field-relative drive command driven by a SwerveInputStream. */
    frc2::CommandPtr DriveFieldRelativeCommand(
        yams::mechanisms::swerve::utility::SwerveInputStream<4>& inputStream)
    {
        return Run([&] {
            swerveDrive_.SetFieldRelativeChassisSpeeds(inputStream.Get());
        }).WithName("Field Oriented Drive");
    }

    void AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
        swerveDrive_.AddVisionMeasurement(pose, timestamp);
    }

    frc::Pose2d GetPose() const { return swerveDrive_.GetPose(); }
    void ZeroGyro() { swerveDrive_.ZeroGyro(); }

    void Periodic() override { swerveDrive_.UpdateTelemetry(); }
    void SimulationPeriodic() override { swerveDrive_.SimIterate(); }

private:
    // ... motor configs, module configs, modules, drive config ...
    yams::mechanisms::swerve::SwerveDrive<4> swerveDrive_{&driveConfig_};
};
```

{% endstep %}

{% step %}

#### Set up driver input with `SwerveInputStream`

`SwerveInputStream<4>` translates raw joystick axes into `frc::ChassisSpeeds`. Pass the `SwerveDrive` reference as the first argument, then set a rotation axis with `WithControllerRotationAxis`.

```cpp
// In RobotContainer or a drive command factory:
yams::mechanisms::swerve::utility::SwerveInputStream<4> driverInput =
    yams::mechanisms::swerve::utility::SwerveInputStream<4>::Of(
        driveSubsystem_->GetSwerveDrive(),
        [&] { return -driverController_.GetLeftY(); },   // forward/back
        [&] { return -driverController_.GetLeftX(); }    // strafe
    )
    .WithControllerRotationAxis([&] { return -driverController_.GetRightX(); })
    .WithDeadband(0.1)
    .WithMaximumLinearVelocity(4.5_mps)
    .WithMaximumAngularVelocity(units::radians_per_second_t{2 * std::numbers::pi})
    .WithAllianceRelativeControl()
    .WithCubeTranslationControllerAxis();

driveSubsystem_->SetDefaultCommand(
    driveSubsystem_->DriveFieldRelativeCommand(driverInput)
);
```

{% endstep %}

{% step %}

#### Heading-snap mode (optional)

Clone the input stream to add heading control without affecting the base stream:

```cpp
yams::mechanisms::swerve::utility::SwerveInputStream<4> headingInput =
    driverInput.Clone()
    .WithControllerHeadingAxis(
        [&] { return driverController_.GetRightX(); },
        [&] { return driverController_.GetRightY(); }
    )
    .WithHeadingControl([&] { return driverController_.GetRightStickButton(); });
```

{% endstep %}

{% step %}

#### Azimuth encoder seeding

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

```cpp
frontLeft.SeedAzimuthEncoder();
frontRight.SeedAzimuthEncoder();
backLeft.SeedAzimuthEncoder();
backRight.SeedAzimuthEncoder();
```

{% endstep %}

{% step %}

#### Auto-align (`DriveToPose`) and live PID tuning

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

```cpp
frc2::CommandPtr DriveSubsystem::DriveToPoseCommand(frc::Pose2d target) {
    return swerveDrive_.DriveToPose(target);
}
```

`SwerveDrive<N>` also publishes a live-tuning command to SmartDashboard at `Mechanisms/<name>/tuning/driveToPose`. Running it checks a tunable `autoalign/enabled` boolean and, while `true`, drives toward a target pose (`autoalign/setpoint/x`/`autoalign/setpoint/y` in meters, `autoalign/setpoint/rot` in degrees) you can edit live in NetworkTables/Glass, so you can tune those PID gains without redeploying code. The same command also reads a shared set of module drive/azimuth PID, feedforward, and setpoint fields (`modules/drive/*`, `modules/azimuth/*` — PID gains under `feedback/`, `kS`/`kV`/`kA` under `feedforward/`) that apply to every module on the drive at once. These are all already live-tunable at `TelemetryVerbosity::HIGH` (the default); see [SwerveDrive (C++): Auto-Align](/api-reference/c++-reference/swerve/swerve-drive.md#auto-align-drive-to-pose--pid-control).
{% endstep %}
{% endstepper %}

***

## Key SwerveDrive methods

| Method                                                               | Description                                                                             |
| -------------------------------------------------------------------- | --------------------------------------------------------------------------------------- |
| `SetFieldRelativeChassisSpeeds(ChassisSpeeds)`                       | Field-relative drive                                                                    |
| `SetRobotRelativeChassisSpeeds(ChassisSpeeds)`                       | Robot-relative drive                                                                    |
| `Drive(std::function<ChassisSpeeds()>)`                              | Returns a run `CommandPtr` for continuous driving                                       |
| `DriveToPose(Pose2d)`                                                | Returns a `CommandPtr` that PID-drives to a field-relative pose                         |
| `DriveToPoseSetpoint(Pose2d)`                                        | Computes one loop's `ChassisSpeeds` toward a pose (building block behind `DriveToPose`) |
| `SetTranslationPID(PIDController)` / `SetRotationPID(PIDController)` | Replace the auto-align PID controllers (integrator preserved unless gains changed)      |
| `ResetTranslationPID()` / `ResetAzimuthPID()`                        | Reset the auto-align PID controllers' internal state                                    |
| `LockPose()`                                                         | X-pattern to resist pushing                                                             |
| `GetPose()`                                                          | Current field-relative pose from odometry                                               |
| `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

{% hint style="warning" %}
`WithSubsystem(this)` must be set on every `SmartMotorControllerConfig` **and** on `SwerveDriveConfig` before construction. Without it, command scheduling will not work correctly.
{% endhint %}

{% hint style="info" %}
`SwerveInputStream::Of()` takes translation axes only. Add rotation later with `WithControllerRotationAxis()` or switch to heading control with `WithControllerHeadingAxis()` + `WithHeadingControl()`.
{% endhint %}

***

## Examples

Complete C++ implementations from the `cpptest` reference project:

{% @github-files/github-code-block url="<https://github.com/Yet-Another-Software-Suite/YAMS/blob/master/examples/cpptest/src/main/cpp/subsystems/SwerveSubsystem.cpp>" %}

***

## Related pages

* [SwerveDrive (C++)](/api-reference/c++-reference/swerve/swerve-drive.md)
* [SwerveModule (C++)](/api-reference/c++-reference/swerve/swerve-module.md)
* [SwerveInputStream (C++)](/api-reference/c++-reference/swerve/swerve-input-stream.md)
* [SmartMotorControllerConfig (C++)](/api-reference/c++-reference/motor-controllers/smart-motor-controller-config.md)
* [Java equivalent: Setting Up Swerve Drive](/api-reference/guides/setting-up-swerve.md)