Package com.ctre.phoenix6.swerve

Class SwerveDrivetrain

java.lang.Object
com.ctre.phoenix6.swerve.SwerveDrivetrain
All Implemented Interfaces:
AutoCloseable
public class SwerveDrivetrain extends Object implements AutoCloseable
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 using the Swerve Project 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.

This class performs pose estimation internally using a separate odometry thread. Vision measurements can be added using addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d, double). Other odometry APIs such as resetPose(edu.wpi.first.math.geometry.Pose2d) are also available. The resulting pose estimate can be retrieved along with module states and other information using getState(). Additionally, the odometry thread synchronously provides all new state updates to a telemetry function registered with registerTelemetry(java.util.function.Consumer<com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState>).

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 third (0-indexed) module.

  • Field Details

    • kNumConfigAttempts

      protected static final int kNumConfigAttempts
      Number of times to attempt config applies.
      See Also:

    • m_drivetrainId

      protected final int m_drivetrainId
      ID of the native drivetrain instance, used for JNI calls.

    • m_jni

      protected final SwerveJNI m_jni
      JNI instance to use for non-static JNI calls. This object is not thread-safe.

    • m_telemetryJNI

      protected final SwerveJNI m_telemetryJNI
      JNI instance to use for telemetry JNI calls. This object is not thread-safe.

  • Constructor Details

    • SwerveDrivetrain

      public SwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules)
      Constructs a CTRE SwerveDrivetrain using the specified constants.

      This constructs the underlying hardware devices, so users 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 CTRE SwerveDrivetrain using the specified constants.

      This constructs the underlying hardware devices, so users 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 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

    • SwerveDrivetrain

      public SwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, Matrix<N3,N1> odometryStandardDeviation, Matrix<N3,N1> visionStandardDeviation, SwerveModuleConstants... modules)
      Constructs a CTRE SwerveDrivetrain using the specified constants.

      This constructs the underlying hardware devices, so users 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 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

  • Method Details

    • close

      public void close()
      Specified by:
      close in interface AutoCloseable

    • 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

    • isOnCANFD

      public final boolean isOnCANFD()
      Gets whether the drivetrain is on a CAN FD bus.
      Returns:
      true if on CAN FD

    • getOdometryFrequency

      public final double getOdometryFrequency()
      Gets the target odometry update frequency in Hz.
      Returns:
      Target odometry update frequency

    • getOdometryFrequencyMeasure

      public final edu.wpi.first.units.measure.Frequency getOdometryFrequencyMeasure()
      Gets the target odometry update frequency.
      Returns:
      Target odometry update frequency

    • getOdometryThread

      public final SwerveDrivetrain.OdometryThread getOdometryThread()
      Gets a reference to the odometry thread.
      Returns:
      Odometry thread

    • isOdometryValid

      public boolean isOdometryValid()
      Check if the odometry is currently valid.
      Returns:
      True if odometry is valid

    • getKinematics

      public final SwerveDriveKinematics getKinematics()
      Gets a reference to the kinematics used for the drivetrain.
      Returns:
      Swerve kinematics

    • setControl

      public void setControl(SwerveRequest request)
      Applies the specified control request to this swerve drivetrain.
      Parameters:
      request - Request to apply

    • getState

      public final SwerveDrivetrain.SwerveDriveState getState()
      Gets the current state of the swerve drivetrain. This includes information such as the pose estimate, module states, and chassis speeds.
      Returns:
      Current state of the drivetrain

    • getStateCopy

      public final SwerveDrivetrain.SwerveDriveState getStateCopy()
      Gets a copy of the current state of the swerve drivetrain. This includes information such as the pose estimate, module states, and chassis speeds.

      This can be used to get a thread-safe copy of the state object.

      Returns:
      Copy of the current state of the drivetrain

    • 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. Additionally, the SwerveDriveState object can be cloned and stored for later processing.

      Parameters:
      telemetryFunction - Function to call for telemetry or logging

    • 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

    • seedFieldCentric

      public void seedFieldCentric()
      Resets the rotation of the robot pose to 0 from the SwerveRequest.ForwardPerspectiveValue.OperatorPerspective perspective. This makes the current orientation of the robot X forward for field-centric maneuvers.

      This is equivalent to calling resetRotation(edu.wpi.first.math.geometry.Rotation2d) with the operator perspective rotation.

    • resetPose

      public void resetPose(Pose2d pose)
      Resets the pose of the robot. The pose should be from the SwerveRequest.ForwardPerspectiveValue.BlueAlliance perspective.
      Parameters:
      pose - Pose to make the current pose

    • resetTranslation

      public void resetTranslation(Translation2d translation)
      Resets the translation of the robot pose without affecting rotation. The translation should be from the SwerveRequest.ForwardPerspectiveValue.BlueAlliance perspective.
      Parameters:
      translation - Translation to make the current translation

    • resetRotation

      public void resetRotation(Rotation2d rotation)
      Resets the rotation of the robot pose without affecting translation. The rotation should be from the SwerveRequest.ForwardPerspectiveValue.BlueAlliance perspective.
      Parameters:
      rotation - Rotation to make the current rotation

    • setOperatorPerspectiveForward

      public void setOperatorPerspectiveForward(Rotation2d fieldDirection)
      Takes the SwerveRequest.ForwardPerspectiveValue.BlueAlliance perpective direction and treats it as the forward direction for SwerveRequest.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 SwerveRequest.ForwardPerspectiveValue.BlueAlliance perspective. As a result, the robot pose may need to be reset using resetPose(edu.wpi.first.math.geometry.Pose2d).

      Parameters:
      fieldDirection - Heading indicating which direction is forward from the SwerveRequest.ForwardPerspectiveValue.BlueAlliance perspective

    • getOperatorForwardDirection

      public final Rotation2d getOperatorForwardDirection()
      Returns the SwerveRequest.ForwardPerspectiveValue.BlueAlliance perpective direction that is treated as the forward direction for SwerveRequest.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.

      Returns:
      Heading indicating which direction is forward from the SwerveRequest.ForwardPerspectiveValue.BlueAlliance perspective

    • 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 PoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, T) 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 you must use a timestamp with an epoch since system startup (i.e., the epoch of this timestamp is the same epoch as Utils.getCurrentTimeSeconds()). This means that you should use Utils.getCurrentTimeSeconds() as your time source or sync the epochs. An FPGA timestamp can be converted to the correct timebase using Utils.fpgaToCurrentTime(double).

    • 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 PoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, T) 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 you must use a timestamp with an epoch since system startup (i.e., the epoch of this timestamp is the same epoch as Utils.getCurrentTimeSeconds()). This means that you should use Utils.getCurrentTimeSeconds() as your time source or sync the epochs. An FPGA timestamp can be converted to the correct timebase using Utils.fpgaToCurrentTime(double).
      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.

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

    • samplePoseAt

      public Optional<Pose2d> samplePoseAt(double timestampSeconds)
      Return the pose at a given timestamp, if the buffer is not empty.
      Parameters:
      timestampSeconds - The pose's timestamp. Note that you must use a timestamp with an epoch since system startup (i.e., the epoch of this timestamp is the same epoch as Utils.getCurrentTimeSeconds()). This means that you should use Utils.getCurrentTimeSeconds() as your time source in this case. An FPGA timestamp can be converted to the correct timebase using Utils.fpgaToCurrentTime(double).
      Returns:
      The pose at the given timestamp (or Optional.empty() if the buffer is empty).

    • getModule

      public final 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

    • getModules

      public final SwerveModule[] getModules()
      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

    • getModuleLocations

      public final Translation2d[] getModuleLocations()
      Gets the locations of the swerve modules.
      Returns:
      Reference to the array of swerve module locations

    • getRotation3d

      public final 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

    • getPigeon2

      public final 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