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
static 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 StatusSignal<Double>	m_angularVelocity
protected SwerveDrivetrain.SwerveDriveState	m_cachedState
protected Rotation2d	m_fieldRelativeOffset
protected SwerveDriveKinematics	m_kinematics
protected Translation2d[]	m_moduleLocations
protected SwerveModulePosition[]	m_modulePositions
protected SwerveModuleState[]	m_moduleStates
protected SwerveDrivePoseEstimator	m_odometry
protected SwerveDrivetrain.OdometryThread	m_odometryThread
protected Rotation2d	m_operatorForwardDirection
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 StatusSignal<Double>	m_yawGetter
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, double OdometryUpdateFrequency, Matrix<N3,N1> odometryStandardDeviation, Matrix<N3,N1> visionStandardDeviation, 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)
StatusCode	configNeutralMode(NeutralModeValue neutralMode)	Configures the neutral mode to use for all modules' drive motors.
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.
Pigeon2	getPigeon2()	Gets this drivetrain's Pigeon 2 reference.
Rotation3d	getRotation3d()	Gets the current orientation of the robot as a Rotation3d from the Pigeon 2 quaternion values.
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	setOperatorPerspectiveForward(Rotation2d fieldDirection)	Takes the SwerveRequest.ForwardReference.RedAlliance perpective direction and treats it as the forward direction for SwerveRequest.ForwardReference.OperatorPerspective.
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

IsOnCANFD

protected final boolean IsOnCANFD

UpdateFrequency

protected final double UpdateFrequency

ModuleCount

protected final int ModuleCount

Modules

protected final SwerveModule[] Modules

m_pigeon2

protected final Pigeon2 m_pigeon2

m_yawGetter

protected final StatusSignal<Double> m_yawGetter

m_angularVelocity

protected final StatusSignal<Double> m_angularVelocity

m_kinematics

protected SwerveDriveKinematics m_kinematics

m_odometry

protected SwerveDrivePoseEstimator m_odometry

m_modulePositions

protected SwerveModulePosition[] m_modulePositions

m_moduleStates

protected SwerveModuleState[] m_moduleStates

m_moduleLocations

protected Translation2d[] m_moduleLocations

m_odometryThread

protected SwerveDrivetrain.OdometryThread m_odometryThread

m_fieldRelativeOffset

protected Rotation2d m_fieldRelativeOffset

m_operatorForwardDirection

protected Rotation2d m_operatorForwardDirection

m_requestToApply

protected SwerveRequest m_requestToApply

m_requestParameters

protected SwerveRequest.SwerveControlRequestParameters m_requestParameters

m_stateLock

protected final ReadWriteLock m_stateLock

m_simDrive

protected final SimSwerveDrivetrain m_simDrive

m_telemetryFunction

protected Consumer<SwerveDrivetrain.SwerveDriveState> m_telemetryFunction

m_cachedState

protected final 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 on CAN FD, and 100 Hz on CAN 2.0.

modules - Constants for each specific module

SwerveDrivetrain

public SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, Matrix<N3,N1> odometryStandardDeviation, Matrix<N3,N1> visionStandardDeviation, 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 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

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

configNeutralMode

public StatusCode configNeutralMode(NeutralModeValue neutralMode)

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

Parameters:

neutralMode - The drive motor neutral mode

Returns:

Status code of the first failed config call, or OK if all succeeded

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.

setOperatorPerspectiveForward

public void setOperatorPerspectiveForward(Rotation2d fieldDirection)

Takes the SwerveRequest.ForwardReference.RedAlliance perpective direction and treats it as the forward direction for 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:

fieldDirection - Heading indicating which direction is forward from the SwerveRequest.ForwardReference.RedAlliance perspective.

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

getRotation3d

public Rotation3d getRotation3d()

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

Returns:

The robot orientation as a Rotation3d

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 PoseEstimator.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

getPigeon2

public Pigeon2 getPigeon2()

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