Package com.ctre.phoenix6.mechanisms.swerve

Class SwerveDrivetrain

java.lang.Object

com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain

public class SwerveDrivetrain
extends Object

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.

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
class	SwerveDrivetrain.OdometryThread
class	SwerveDrivetrain.SwerveDriveState	Plain-Old-Data class holding the state of the swerve drivetrain.

Field Summary

Fields

Modifier and Type	Field	Description
protected boolean	IsOnCANFD
protected SwerveDrivetrain.SwerveDriveState	m_cachedState
protected Rotation2d	m_fieldRelativeOffset
protected SwerveDriveKinematics	m_kinematics
protected Translation2d[]	m_moduleLocations
protected SwerveModulePosition[]	m_modulePositions
protected SwerveDrivePoseEstimator	m_odometry
protected SwerveDrivetrain.OdometryThread	m_odometryThread
protected Pigeon2	m_pigeon2
protected SwerveRequest.SwerveControlRequestParameters	m_requestParameters
protected SwerveRequest	m_requestToApply
protected SimSwerveDrivetrain	m_simDrive
protected ReadWriteLock	m_stateLock
protected Consumer<SwerveDrivetrain.SwerveDriveState>	m_telemetryFunction
protected int	ModuleCount
protected SwerveModule[]	Modules
protected double	UpdateFrequency

Constructor Summary

Constructors

Constructor	Description
SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules)	Constructs a CTRSwerveDrivetrain using the specified constants.
SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules)	Constructs a CTRSwerveDrivetrain using the specified constants.

Method Summary

Modifier and Type	Method	Description
void	addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds)	Adds a vision measurement to the Kalman Filter.
void	addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)	Adds a vision measurement to the Kalman Filter.
protected boolean	checkIsOnCanFD(String canbusName)
SwerveDrivetrain.OdometryThread	getDaqThread()	Gets a reference to the data acquisition thread.
SwerveModule	getModule(int index)	Get a reference to the module at the specified index.
SwerveDrivetrain.SwerveDriveState	getState()	Gets the current state of the swerve drivetrain.
boolean	odometryIsValid()	Check if the odometry is currently valid
void	registerTelemetry(Consumer<SwerveDrivetrain.SwerveDriveState> telemetryFunction)	Register the specified lambda to be executed whenever our SwerveDriveState function is updated in our odometry thread.
void	seedFieldRelative()	Takes the current orientation of the robot and makes it X forward for field-relative maneuvers.
void	seedFieldRelative(Pose2d location)	Takes the specified location and makes it the current pose for field-relative maneuvers
void	setControl(SwerveRequest request)	Applies the specified control request to this swerve drivetrain.
void	setVisionMeasurementStdDevs(Matrix<N3,N1> visionMeasurementStdDevs)	Sets the pose estimator's trust of global measurements.
void	tareEverything()	Zero's this swerve drive's odometry entirely.
void	updateSimState(double dtSeconds, double supplyVoltage)	Updates all the simulation state variables for this drivetrain class.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Details

ModuleCount

protected final int ModuleCount

UpdateFrequency

protected final double UpdateFrequency

Modules

protected final SwerveModule[] Modules

m_pigeon2

protected Pigeon2 m_pigeon2

m_kinematics

protected SwerveDriveKinematics m_kinematics

m_odometry

protected SwerveDrivePoseEstimator m_odometry

m_modulePositions

protected SwerveModulePosition[] m_modulePositions

m_moduleLocations

protected Translation2d[] m_moduleLocations

m_odometryThread

protected SwerveDrivetrain.OdometryThread m_odometryThread

m_fieldRelativeOffset

protected Rotation2d m_fieldRelativeOffset

m_requestToApply

protected SwerveRequest m_requestToApply

m_requestParameters

protected SwerveRequest.SwerveControlRequestParameters m_requestParameters

m_stateLock

protected ReadWriteLock m_stateLock

m_simDrive

protected final SimSwerveDrivetrain m_simDrive

IsOnCANFD

protected final boolean IsOnCANFD

m_telemetryFunction

protected Consumer<SwerveDrivetrain.SwerveDriveState> m_telemetryFunction

m_cachedState

protected SwerveDrivetrain.SwerveDriveState m_cachedState

Constructor Details

SwerveDrivetrain

public SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules)

Constructs a CTRSwerveDrivetrain 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:

driveTrainConstants - Drivetrain-wide constants for the swerve drive

modules - Constants for each specific module

SwerveDrivetrain

public SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules)

Constructs a CTRSwerveDrivetrain 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:

driveTrainConstants - Drivetrain-wide constants for the swerve drive

OdometryUpdateFrequency - The frequency to run the odometry loop. If unspecified, this is 250 hz.

modules - Constants for each specific module

Method Details

checkIsOnCanFD

protected boolean checkIsOnCanFD(String canbusName)

getDaqThread

public SwerveDrivetrain.OdometryThread getDaqThread()

Gets a reference to the data acquisition thread.

Returns:

DAQ thread

setControl

public void setControl(SwerveRequest request)

Applies the specified control request to this swerve drivetrain.

Parameters:

request - Request to apply

tareEverything

public void tareEverything()

Zero's this swerve drive's odometry entirely.

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

seedFieldRelative

public void seedFieldRelative()

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

seedFieldRelative

public void seedFieldRelative(Pose2d location)

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

Parameters:

location - Pose to make the current pose at.

odometryIsValid

public boolean odometryIsValid()

Check if the odometry is currently valid

Returns:

True if odometry is valid

getModule

public SwerveModule getModule(int index)

Get a reference to the module at the specified index. The index corresponds to the module described in the constructor

Parameters:

index - Which module to get

Returns:

Reference to SwerveModule

getState

public SwerveDrivetrain.SwerveDriveState getState()

Gets the current state of the swerve drivetrain.

Returns:

Current state of the drivetrain

addVisionMeasurement

public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)

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 SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) 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 SwerveDrivePoseEstimator.setVisionMeasurementStdDevs(Matrix) or this method.

Parameters:

visionRobotPoseMeters - The pose of the robot as measured by the vision camera.

timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling SwerveDrivePoseEstimator.updateWithTime(double,Rotation2d,SwerveModulePosition[]), then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as Timer.getFPGATimestamp()). This means that you should use Timer.getFPGATimestamp() 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.

addVisionMeasurement

public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds)

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 SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) 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:

visionRobotPoseMeters - The pose of the robot as measured by the vision camera.

timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling SwerveDrivePoseEstimator.updateWithTime(double,Rotation2d,SwerveModulePosition[]) then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as Timer.getFPGATimestamp().) This means that you should use Timer.getFPGATimestamp() as your time source or sync the epochs.

setVisionMeasurementStdDevs

public void setVisionMeasurementStdDevs(Matrix<N3,N1> visionMeasurementStdDevs)

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:

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.

updateSimState

public void updateSimState(double dtSeconds, double supplyVoltage)

Updates all the simulation state variables for this drivetrain class. User provides the update variables for the simulation.

Parameters:

dtSeconds - time since last update call

supplyVoltage - voltage as seen at the motor controllers

registerTelemetry

public void registerTelemetry(Consumer<SwerveDrivetrain.SwerveDriveState> telemetryFunction)

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

It is imperative that this function is cheap, as it will be executed along with the odometry call, and 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:

telemetryFunction - Function to call for telemetry or logging