CTRE Phoenix 6 C++ 24.50.0-alpha-2
ctre::phoenix6::swerve::SwerveDrivetrain Class Reference

Swerve Drive class utilizing CTR Electronics Phoenix 6 API. More...

#include <ctre/phoenix6/swerve/SwerveDrivetrain.hpp>

Public Types

using OdometryThread = impl::SwerveDrivetrainImpl::OdometryThread
 Performs swerve module updates in a separate thread to minimize latency. More...
 
using SwerveDriveState = impl::SwerveDrivetrainImpl::SwerveDriveState
 Plain-Old-Data class holding the state of the swerve drivetrain. More...
 

Public Member Functions

template<typename... ModuleConstants, typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> >>
 SwerveDrivetrain (SwerveDrivetrainConstants const &drivetrainConstants, ModuleConstants const &... modules)
 Constructs a CTRE SwerveDrivetrain using the specified constants. More...
 
template<typename... ModuleConstants, typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> >>
 SwerveDrivetrain (SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, ModuleConstants const &... modules)
 Constructs a CTRE SwerveDrivetrain using the specified constants. More...
 
template<typename... ModuleConstants, typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> >>
 SwerveDrivetrain (SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, std::array< double, 3 > const &odometryStandardDeviation, std::array< double, 3 > const &visionStandardDeviation, ModuleConstants const &... modules)
 Constructs a CTRE SwerveDrivetrain using the specified constants. More...
 
virtual ~SwerveDrivetrain ()=default
 
void UpdateSimState (units::second_t dt, units::volt_t supplyVoltage)
 Updates all the simulation state variables for this drivetrain class. More...
 
OdometryThreadGetDaqThread ()
 Gets a reference to the data acquisition thread. More...
 
template<typename Request , typename = std::enable_if_t< std::is_base_of_v<requests::SwerveRequest, std::remove_reference_t<Request>> >>
void SetControl (Request &&request)
 Applies the specified control request to this swerve drivetrain. More...
 
ctre::phoenix::StatusCode ConfigNeutralMode (signals::NeutralModeValue neutralMode)
 Configures the neutral mode to use for all modules' drive motors. More...
 
void TareEverything ()
 Zero's this swerve drive's odometry entirely. More...
 
void SeedFieldRelative ()
 Takes the current orientation of the robot and makes it X forward for field-relative maneuvers. More...
 
void SeedFieldRelative (frc::Pose2d location)
 Takes the specified location and makes it the current pose for field-relative maneuvers. More...
 
void SetOperatorPerspectiveForward (frc::Rotation2d fieldDirection)
 Takes the requests::SwerveRequest::ForwardReference::RedAlliance perpective direction and treats it as the forward direction for requests::SwerveRequest::ForwardReference::OperatorPerspective. More...
 
bool IsOdometryValid () const
 Check if the odometry is currently valid. More...
 
SwerveModuleGetModule (size_t index)
 Get a reference to the module at the specified index. More...
 
SwerveModule const & GetModule (size_t index) const
 Get a reference to the module at the specified index. More...
 
std::vector< std::unique_ptr< SwerveModule > > const & GetModules () const
 Get a reference to the full array of modules. More...
 
std::vector< Translation2d > const & GetModuleLocations () const
 Gets the locations of the swerve modules. More...
 
SwerveDriveState GetState () const
 Gets the current state of the swerve drivetrain. More...
 
void AddVisionMeasurement (frc::Pose2d visionRobotPose, units::second_t timestamp)
 Adds a vision measurement to the Kalman Filter. More...
 
void AddVisionMeasurement (frc::Pose2d visionRobotPose, units::second_t timestamp, std::array< double, 3 > visionMeasurementStdDevs)
 Adds a vision measurement to the Kalman Filter. More...
 
void SetVisionMeasurementStdDevs (std::array< double, 3 > visionMeasurementStdDevs)
 Sets the pose estimator's trust of global measurements. More...
 
void RegisterTelemetry (std::function< void(SwerveDriveState const &)> telemetryFunction)
 Register the specified lambda to be executed whenever the SwerveDriveState is updated in the odometry thread. More...
 
frc::Rotation3d GetRotation3d () const
 Gets the current orientation of the robot as a frc::Rotation3d from the Pigeon 2 quaternion values. More...
 
hardware::Pigeon2GetPigeon2 ()
 Gets this drivetrain's Pigeon 2 reference. More...
 

Protected Attributes

impl::SwerveDrivetrainImpl _drivetrain
 
std::vector< std::unique_ptr< SwerveModule > > _modules
 
hardware::Pigeon2 _pigeon2
 
SimSwerveDrivetrain _simDrive
 

Detailed Description

Swerve Drive class utilizing CTR Electronics Phoenix 6 API.

This class handles the kinematics, configuration, and odometry of a swerve drive utilizing CTR Electronics devices. We recommend that users use the Swerve Mechanism Generator in Tuner X to create a template project that demonstrates how to use this class.

This class will construct the hardware devices internally, so the user only specifies the constants (IDs, PID gains, gear ratios, etc). Getters for these hardware devices are available.

If using the generator, the order in which modules are constructed is Front Left, Front Right, Back Left, Back Right. This means if you need the Back Left module, call GetModule(2); to get the 3rd index (0-indexed) module, corresponding to the Back Left module.

Member Typedef Documentation

◆ OdometryThread

Performs swerve module updates in a separate thread to minimize latency.

◆ SwerveDriveState

Plain-Old-Data class holding the state of the swerve drivetrain.

This encapsulates most data that is relevant for telemetry or decision-making from the Swerve Drive.

Constructor & Destructor Documentation

◆ SwerveDrivetrain() [1/3]

template<typename... ModuleConstants, typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> >>
ctre::phoenix6::swerve::SwerveDrivetrain::SwerveDrivetrain ( SwerveDrivetrainConstants const &  drivetrainConstants,
ModuleConstants const &...  modules 
)
inline

Constructs a CTRE SwerveDrivetrain using the specified constants.

This constructs the underlying hardware devices, so user should not construct the devices themselves. If they need the devices, they can access them through getters in the classes.

Parameters
drivetrainConstantsDrivetrain-wide constants for the swerve drive
modulesConstants for each specific module

◆ SwerveDrivetrain() [2/3]

template<typename... ModuleConstants, typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> >>
ctre::phoenix6::swerve::SwerveDrivetrain::SwerveDrivetrain ( SwerveDrivetrainConstants const &  drivetrainConstants,
units::hertz_t  odometryUpdateFrequency,
ModuleConstants const &...  modules 
)
inline

Constructs a CTRE SwerveDrivetrain using the specified constants.

This constructs the underlying hardware devices, so user should not construct the devices themselves. If they need the devices, they can access them through getters in the classes.

Parameters
drivetrainConstantsDrivetrain-wide constants for the swerve drive
odometryUpdateFrequencyThe frequency to run the odometry loop. If unspecified or set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
modulesConstants for each specific module

◆ SwerveDrivetrain() [3/3]

template<typename... ModuleConstants, typename = std::enable_if_t< std::conjunction_v<std::is_same<ModuleConstants, SwerveModuleConstants>...> >>
ctre::phoenix6::swerve::SwerveDrivetrain::SwerveDrivetrain ( SwerveDrivetrainConstants const &  drivetrainConstants,
units::hertz_t  odometryUpdateFrequency,
std::array< double, 3 > const &  odometryStandardDeviation,
std::array< double, 3 > const &  visionStandardDeviation,
ModuleConstants const &...  modules 
)
inline

Constructs a CTRE SwerveDrivetrain using the specified constants.

This constructs the underlying hardware devices, so user should not construct the devices themselves. If they need the devices, they can access them through getters in the classes.

Parameters
drivetrainConstantsDrivetrain-wide constants for the swerve drive
odometryUpdateFrequencyThe frequency to run the odometry loop. If unspecified or set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
odometryStandardDeviationThe standard deviation for odometry calculation
visionStandardDeviationThe standard deviation for vision calculation
modulesConstants for each specific module

◆ ~SwerveDrivetrain()

virtual ctre::phoenix6::swerve::SwerveDrivetrain::~SwerveDrivetrain ( )
virtualdefault

Member Function Documentation

◆ AddVisionMeasurement() [1/2]

void ctre::phoenix6::swerve::SwerveDrivetrain::AddVisionMeasurement ( frc::Pose2d  visionRobotPose,
units::second_t  timestamp 
)
inline

Adds a vision measurement to the Kalman Filter.

This will correct the odometry pose estimate while still accounting for measurement noise.

This method can be called as infrequently as you want, as long as you are calling impl::SwerveDrivePoseEstimator::Update every loop.

To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.

Parameters
visionRobotPoseThe pose of the robot as measured by the vision camera.
timestampThe timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling impl::SwerveDrivePoseEstimator::UpdateWithTime, then you must use a timestamp with an epoch since system startup (i.e., the epoch of this timestamp is the same epoch as GetCurrentTimeSeconds). This means that you should use GetCurrentTimeSeconds as your time source in this case.

◆ AddVisionMeasurement() [2/2]

void ctre::phoenix6::swerve::SwerveDrivetrain::AddVisionMeasurement ( frc::Pose2d  visionRobotPose,
units::second_t  timestamp,
std::array< double, 3 >  visionMeasurementStdDevs 
)
inline

Adds a vision measurement to the Kalman Filter.

This will correct the odometry pose estimate while still accounting for measurement noise.

This method can be called as infrequently as you want, as long as you are calling impl::SwerveDrivePoseEstimator::Update every loop.

To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.

Note that the vision measurement standard deviations passed into this method will continue to apply to future measurements until a subsequent call to SetVisionMeasurementStdDevs or this method.

Parameters
visionRobotPoseThe pose of the robot as measured by the vision camera.
timestampThe timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling impl::SwerveDrivePoseEstimator::UpdateWithTime, then you must use a timestamp with an epoch since system startup (i.e., the epoch of this timestamp is the same epoch as GetCurrentTimeSeconds). This means that you should use GetCurrentTimeSeconds as your time source in this case.
visionMeasurementStdDevsStandard deviations of the vision pose measurement (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.

◆ ConfigNeutralMode()

ctre::phoenix::StatusCode ctre::phoenix6::swerve::SwerveDrivetrain::ConfigNeutralMode ( signals::NeutralModeValue  neutralMode)
inline

Configures the neutral mode to use for all modules' drive motors.

Parameters
neutralModeThe drive motor neutral mode
Returns
Status code of the first failed config call, or OK if all succeeded

◆ GetDaqThread()

OdometryThread & ctre::phoenix6::swerve::SwerveDrivetrain::GetDaqThread ( )
inline

Gets a reference to the data acquisition thread.

Returns
DAQ thread

◆ GetModule() [1/2]

SwerveModule & ctre::phoenix6::swerve::SwerveDrivetrain::GetModule ( size_t  index)
inline

Get a reference to the module at the specified index.

The index corresponds to the module described in the constructor.

Parameters
indexWhich module to get
Returns
Reference to SwerveModule

◆ GetModule() [2/2]

SwerveModule const & ctre::phoenix6::swerve::SwerveDrivetrain::GetModule ( size_t  index) const
inline

Get a reference to the module at the specified index.

The index corresponds to the module described in the constructor.

Parameters
indexWhich module to get
Returns
Reference to SwerveModule

◆ GetModuleLocations()

std::vector< Translation2d > const & ctre::phoenix6::swerve::SwerveDrivetrain::GetModuleLocations ( ) const
inline

Gets the locations of the swerve modules.

Returns
Reference to the array of swerve module locations

◆ GetModules()

std::vector< std::unique_ptr< SwerveModule > > const & ctre::phoenix6::swerve::SwerveDrivetrain::GetModules ( ) const
inline

Get a reference to the full array of modules.

The indexes correspond to the module described in the constructor.

Returns
Reference to the SwerveModule array

◆ GetPigeon2()

hardware::Pigeon2 & ctre::phoenix6::swerve::SwerveDrivetrain::GetPigeon2 ( )
inline

Gets this drivetrain's Pigeon 2 reference.

This should be used only to access signals and change configurations that the swerve drivetrain does not configure itself.

Returns
This drivetrain's Pigeon 2 reference

◆ GetRotation3d()

frc::Rotation3d ctre::phoenix6::swerve::SwerveDrivetrain::GetRotation3d ( ) const
inline

Gets the current orientation of the robot as a frc::Rotation3d from the Pigeon 2 quaternion values.

Returns
The robot orientation as a frc::Rotation3d

◆ GetState()

SwerveDriveState ctre::phoenix6::swerve::SwerveDrivetrain::GetState ( ) const
inline

Gets the current state of the swerve drivetrain.

Returns
Current state of the drivetrain

◆ IsOdometryValid()

bool ctre::phoenix6::swerve::SwerveDrivetrain::IsOdometryValid ( ) const
inline

Check if the odometry is currently valid.

Returns
True if odometry is valid

◆ RegisterTelemetry()

void ctre::phoenix6::swerve::SwerveDrivetrain::RegisterTelemetry ( std::function< void(SwerveDriveState const &)>  telemetryFunction)
inline

Register the specified lambda to be executed whenever the SwerveDriveState is updated in the odometry thread.

It is imperative that this function is cheap, as it will be executed synchronously with the odometry call; if this takes a long time, it may negatively impact the odometry of this stack.

This can also be used for logging data if the function performs logging instead of telemetry.

Parameters
telemetryFunctionFunction to call for telemetry or logging

◆ SeedFieldRelative() [1/2]

void ctre::phoenix6::swerve::SwerveDrivetrain::SeedFieldRelative ( )
inline

Takes the current orientation of the robot and makes it X forward for field-relative maneuvers.

◆ SeedFieldRelative() [2/2]

void ctre::phoenix6::swerve::SwerveDrivetrain::SeedFieldRelative ( frc::Pose2d  location)
inline

Takes the specified location and makes it the current pose for field-relative maneuvers.

Parameters
locationPose to make the current pose

◆ SetControl()

template<typename Request , typename = std::enable_if_t< std::is_base_of_v<requests::SwerveRequest, std::remove_reference_t<Request>> >>
void ctre::phoenix6::swerve::SwerveDrivetrain::SetControl ( Request &&  request)
inline

Applies the specified control request to this swerve drivetrain.

Parameters
requestRequest to apply

◆ SetOperatorPerspectiveForward()

void ctre::phoenix6::swerve::SwerveDrivetrain::SetOperatorPerspectiveForward ( frc::Rotation2d  fieldDirection)
inline

Takes the requests::SwerveRequest::ForwardReference::RedAlliance perpective direction and treats it as the forward direction for requests::SwerveRequest::ForwardReference::OperatorPerspective.

If the operator is in the Blue Alliance Station, this should be 0 degrees. If the operator is in the Red Alliance Station, this should be 180 degrees.

Parameters
fieldDirectionHeading indicating which direction is forward from the requests::SwerveRequest::ForwardReference::RedAlliance perspective

◆ SetVisionMeasurementStdDevs()

void ctre::phoenix6::swerve::SwerveDrivetrain::SetVisionMeasurementStdDevs ( std::array< double, 3 >  visionMeasurementStdDevs)
inline

Sets the pose estimator's trust of global measurements.

This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases.

Parameters
visionMeasurementStdDevsStandard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.

◆ TareEverything()

void ctre::phoenix6::swerve::SwerveDrivetrain::TareEverything ( )
inline

Zero's this swerve drive's odometry entirely.

This will zero the entire odometry, and place the robot at 0,0

◆ UpdateSimState()

void ctre::phoenix6::swerve::SwerveDrivetrain::UpdateSimState ( units::second_t  dt,
units::volt_t  supplyVoltage 
)
inline

Updates all the simulation state variables for this drivetrain class.

User provides the update variables for the simulation.

Parameters
dttime since last update call
supplyVoltagevoltage as seen at the motor controllers

Member Data Documentation

◆ _drivetrain

impl::SwerveDrivetrainImpl ctre::phoenix6::swerve::SwerveDrivetrain::_drivetrain
protected

◆ _modules

std::vector<std::unique_ptr<SwerveModule> > ctre::phoenix6::swerve::SwerveDrivetrain::_modules
protected

◆ _pigeon2

hardware::Pigeon2 ctre::phoenix6::swerve::SwerveDrivetrain::_pigeon2
protected

◆ _simDrive

SimSwerveDrivetrain ctre::phoenix6::swerve::SwerveDrivetrain::_simDrive
protected

The documentation for this class was generated from the following file: