CTRE Phoenix 6 C++ 24.50.0-alpha-2
|
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... | |
OdometryThread & | GetDaqThread () |
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... | |
SwerveModule & | GetModule (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::Pigeon2 & | GetPigeon2 () |
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 |
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.
using ctre::phoenix6::swerve::SwerveDrivetrain::OdometryThread = impl::SwerveDrivetrainImpl::OdometryThread |
Performs swerve module updates in a separate thread to minimize latency.
using ctre::phoenix6::swerve::SwerveDrivetrain::SwerveDriveState = impl::SwerveDrivetrainImpl::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.
|
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.
drivetrainConstants | Drivetrain-wide constants for the swerve drive |
modules | Constants for each specific module |
|
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.
drivetrainConstants | Drivetrain-wide constants for the swerve drive |
odometryUpdateFrequency | The 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. |
modules | Constants for each specific module |
|
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.
drivetrainConstants | Drivetrain-wide constants for the swerve drive |
odometryUpdateFrequency | The 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. |
odometryStandardDeviation | The standard deviation for odometry calculation |
visionStandardDeviation | The standard deviation for vision calculation |
modules | Constants for each specific module |
|
virtualdefault |
|
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.
visionRobotPose | The pose of the robot as measured by the vision camera. |
timestamp | The 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. |
|
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.
visionRobotPose | The pose of the robot as measured by the vision camera. |
timestamp | The 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. |
visionMeasurementStdDevs | Standard 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. |
|
inline |
Configures the neutral mode to use for all modules' drive motors.
neutralMode | The drive motor neutral mode |
|
inline |
Gets a reference to the data acquisition thread.
|
inline |
Get a reference to the module at the specified index.
The index corresponds to the module described in the constructor.
index | Which module to get |
|
inline |
Get a reference to the module at the specified index.
The index corresponds to the module described in the constructor.
index | Which module to get |
|
inline |
Gets the locations of the swerve modules.
|
inline |
Get a reference to the full array of modules.
The indexes correspond to the module described in the constructor.
|
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.
|
inline |
Gets the current orientation of the robot as a frc::Rotation3d from the Pigeon 2 quaternion values.
|
inline |
Gets the current state of the swerve drivetrain.
|
inline |
Check if the odometry is currently valid.
|
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.
telemetryFunction | Function to call for telemetry or logging |
|
inline |
Takes the current orientation of the robot and makes it X forward for field-relative maneuvers.
|
inline |
Takes the specified location and makes it the current pose for field-relative maneuvers.
location | Pose to make the current pose |
|
inline |
Applies the specified control request to this swerve drivetrain.
request | Request to apply |
|
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.
fieldDirection | Heading indicating which direction is forward from the requests::SwerveRequest::ForwardReference::RedAlliance perspective |
|
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.
visionMeasurementStdDevs | Standard 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. |
|
inline |
Zero's this swerve drive's odometry entirely.
This will zero the entire odometry, and place the robot at 0,0
|
inline |
Updates all the simulation state variables for this drivetrain class.
User provides the update variables for the simulation.
dt | time since last update call |
supplyVoltage | voltage as seen at the motor controllers |
|
protected |
|
protected |
|
protected |
|
protected |