CTRE Phoenix 6 C++ 25.2.1
|
Swerve Drive class utilizing CTR Electronics Phoenix 6 API. More...
#include <ctre/phoenix6/swerve/impl/SwerveDrivetrainImpl.hpp>
Classes | |
struct | ControlParameters |
Contains everything the control requests need to calculate the module state. More... | |
class | OdometryThread |
Performs swerve module updates in a separate thread to minimize latency. More... | |
struct | SwerveDriveState |
Plain-Old-Data class holding the state of the swerve drivetrain. More... | |
Public Types | |
using | SwerveRequestFunc = std::function<ctre::phoenix::StatusCode(ControlParameters const &, std::vector<std::unique_ptr<SwerveModuleImpl>> const &)> |
Public Member Functions | |
template<typename DriveMotorConfigsT , typename SteerMotorConfigsT , typename EncoderConfigsT > | |
SwerveDrivetrainImpl (SwerveDrivetrainConstants const &drivetrainConstants, span< SwerveModuleConstants< DriveMotorConfigsT, SteerMotorConfigsT, EncoderConfigsT > const > modules) | |
Constructs a CTRE SwerveDrivetrainImpl using the specified constants. | |
template<typename DriveMotorConfigsT , typename SteerMotorConfigsT , typename EncoderConfigsT > | |
SwerveDrivetrainImpl (SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, span< SwerveModuleConstants< DriveMotorConfigsT, SteerMotorConfigsT, EncoderConfigsT > const > modules) | |
Constructs a CTRE SwerveDrivetrainImpl using the specified constants. | |
template<typename DriveMotorConfigsT , typename SteerMotorConfigsT , typename EncoderConfigsT > | |
SwerveDrivetrainImpl (SwerveDrivetrainConstants const &drivetrainConstants, units::hertz_t odometryUpdateFrequency, std::array< double, 3 > const &odometryStandardDeviation, std::array< double, 3 > const &visionStandardDeviation, span< SwerveModuleConstants< DriveMotorConfigsT, SteerMotorConfigsT, EncoderConfigsT > const > modules) | |
Constructs a CTRE SwerveDrivetrainImpl using the specified constants. | |
bool | IsOnCANFD () const |
Gets whether the drivetrain is on a CAN FD bus. | |
units::hertz_t | GetOdometryFrequency () const |
Gets the target odometry update frequency. | |
OdometryThread & | GetOdometryThread () |
Gets a reference to the odometry thread. | |
bool | IsOdometryValid () const |
Check if the odometry is currently valid. | |
SwerveDriveKinematics const & | GetKinematics () const |
Gets a reference to the kinematics used for the drivetrain. | |
void | SetControl (SwerveRequestFunc &&request) |
Applies the specified control function to this swerve drivetrain. | |
ctre::phoenix::StatusCode | RunTempRequest (SwerveRequestFunc &&request) const |
Immediately runs the provided temporary control function. | |
SwerveDriveState | GetState () const |
Gets the current state of the swerve drivetrain. | |
void | RegisterTelemetry (std::function< void(SwerveDriveState const &)> telemetryFunction) |
Register the specified lambda to be executed whenever the SwerveDriveState is updated in the odometry thread. | |
ctre::phoenix::StatusCode | ConfigNeutralMode (signals::NeutralModeValue neutralMode) |
Configures the neutral mode to use for all modules' drive motors. | |
void | TareEverything () |
Zero's this swerve drive's odometry entirely. | |
void | SeedFieldCentric () |
Resets the rotation of the robot pose to 0 from the requests::ForwardPerspectiveValue::OperatorPerspective perspective. | |
void | ResetPose (Pose2d const &pose) |
Resets the pose of the robot. | |
void | ResetTranslation (Translation2d const &translation) |
Resets the translation of the robot pose without affecting rotation. | |
void | ResetRotation (Rotation2d const &rotation) |
Resets the rotation of the robot pose without affecting translation. | |
void | SetOperatorPerspectiveForward (Rotation2d fieldDirection) |
Takes the requests::ForwardPerspectiveValue::BlueAlliance perpective direction and treats it as the forward direction for requests::ForwardPerspectiveValue::OperatorPerspective. | |
Rotation2d | GetOperatorForwardDirection () const |
Returns the requests::ForwardPerspectiveValue::BlueAlliance perpective direction that is treated as the forward direction for requests::ForwardPerspectiveValue::OperatorPerspective. | |
void | AddVisionMeasurement (Pose2d visionRobotPose, units::second_t timestamp) |
Adds a vision measurement to the Kalman Filter. | |
void | AddVisionMeasurement (Pose2d visionRobotPose, units::second_t timestamp, std::array< double, 3 > visionMeasurementStdDevs) |
Adds a vision measurement to the Kalman Filter. | |
void | SetVisionMeasurementStdDevs (std::array< double, 3 > visionMeasurementStdDevs) |
Sets the pose estimator's trust of global measurements. | |
std::optional< Pose2d > | SamplePoseAt (units::second_t timestamp) |
Return the pose at a given timestamp, if the buffer is not empty. | |
SwerveModuleImpl & | GetModule (size_t index) |
Get a reference to the module at the specified index. | |
SwerveModuleImpl const & | GetModule (size_t index) const |
Get a reference to the module at the specified index. | |
std::vector< std::unique_ptr< SwerveModuleImpl > > const & | GetModules () const |
Get a reference to the full array of modules. | |
std::vector< Translation2d > const & | GetModuleLocations () const |
Gets the locations of the swerve modules. | |
hardware::core::CorePigeon2 & | GetPigeon2 () |
Gets this drivetrain's Pigeon 2 reference. | |
Friends | |
class | OdometryThread |
Swerve Drive class utilizing CTR Electronics Phoenix 6 API.
This class handles the kinematics and odometry (but not configuration) of a swerve drive utilizing CTR Electronics devices. Users should create a high-level SwerveDrivetrain instead of using this directly.
using ctre::phoenix6::swerve::impl::SwerveDrivetrainImpl::SwerveRequestFunc = std::function<ctre::phoenix::StatusCode(ControlParameters const &, std::vector<std::unique_ptr<SwerveModuleImpl>> const &)> |
|
inline |
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
This constructs the underlying hardware devices, which can be accessed through getters in the classes.
drivetrainConstants | Drivetrain-wide constants for the swerve drive |
modules | Constants for each specific module |
|
inline |
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
This constructs the underlying hardware devices, which can be accessed 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 |
ctre::phoenix6::swerve::impl::SwerveDrivetrainImpl::SwerveDrivetrainImpl | ( | SwerveDrivetrainConstants const & | drivetrainConstants, |
units::hertz_t | odometryUpdateFrequency, | ||
std::array< double, 3 > const & | odometryStandardDeviation, | ||
std::array< double, 3 > const & | visionStandardDeviation, | ||
span< SwerveModuleConstants< DriveMotorConfigsT, SteerMotorConfigsT, EncoderConfigsT > const > | modules ) |
Constructs a CTRE SwerveDrivetrainImpl using the specified constants.
This constructs the underlying hardware devices, which can be accessed 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 in the form [x, y, theta]ᵀ, with units in meters and radians |
visionStandardDeviation | The standard deviation for vision calculation in the form [x, y, theta]ᵀ, with units in meters and radians |
modules | Constants for each specific module |
|
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 utils::GetCurrentTime). This means that you should use utils::GetCurrentTime 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 utils::GetCurrentTime). This means that you should use utils::GetCurrentTime 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 kinematics used for the drivetrain.
|
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 the target odometry update frequency.
|
inline |
Gets a reference to the odometry thread.
|
inline |
Returns the requests::ForwardPerspectiveValue::BlueAlliance perpective direction that is treated as the forward direction for requests::ForwardPerspectiveValue::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.
|
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 state of the swerve drivetrain.
This includes information such as the pose estimate, module states, and chassis speeds.
|
inline |
Check if the odometry is currently valid.
|
inline |
Gets whether the drivetrain is on a CAN FD bus.
|
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. Additionally, the SwerveDriveState object can be cloned and stored for later processing.
telemetryFunction | Function to call for telemetry or logging |
|
inline |
Resets the pose of the robot.
The pose should be from the requests::ForwardPerspectiveValue::BlueAlliance perspective.
pose | Pose to make the current pose |
|
inline |
Resets the rotation of the robot pose without affecting translation.
The rotation should be from the requests::ForwardPerspectiveValue::BlueAlliance perspective.
rotation | Rotation to make the current rotation |
|
inline |
Resets the translation of the robot pose without affecting rotation.
The translation should be from the requests::ForwardPerspectiveValue::BlueAlliance perspective.
translation | Translation to make the current translation |
|
inline |
Immediately runs the provided temporary control function.
This is used to accelerate non-native swerve requests and can only be called from the odometry thread. Otherwise, SetControl should be used instead.
request | Request function to invoke |
|
inline |
Return the pose at a given timestamp, if the buffer is not empty.
timestamp | The pose's timestamp. 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 utils::GetCurrentTime). This means that you should use utils::GetCurrentTime as your time source in this case. |
|
inline |
Resets the rotation of the robot pose to 0 from the requests::ForwardPerspectiveValue::OperatorPerspective perspective.
This makes the current orientation of the robot X forward for field-centric maneuvers.
This is equivalent to calling ResetRotation with the operator perspective rotation.
|
inline |
Applies the specified control function to this swerve drivetrain.
request | Request function to apply |
|
inline |
Takes the requests::ForwardPerspectiveValue::BlueAlliance perpective direction and treats it as the forward direction for requests::ForwardPerspectiveValue::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.
This does not change the robot pose, which is in the requests::ForwardPerspectiveValue::BlueAlliance perspective. As a result, the robot pose may need to be reset using ResetPose.
fieldDirection | Heading indicating which direction is forward from the requests::ForwardPerspectiveValue::BlueAlliance 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
|
friend |