Package com.ctre.phoenix6.mechanisms.swerve

Class LegacySwerveDrivetrain

java.lang.Object
com.ctre.phoenix6.mechanisms.swerve.LegacySwerveDrivetrain
public class LegacySwerveDrivetrain 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.

  • Field Details

    • IsOnCANFD

      protected final boolean IsOnCANFD

    • m_canbus

      protected final CANBus m_canbus

    • UpdateFrequency

      protected final double UpdateFrequency

    • ModuleCount

      protected final int ModuleCount

    • Modules

      protected final LegacySwerveModule[] Modules

    • m_pigeon2

      protected final Pigeon2 m_pigeon2

    • m_yawGetter

      protected final StatusSignal<edu.wpi.first.units.measure.Angle> m_yawGetter

    • m_angularVelocity

      protected final StatusSignal<edu.wpi.first.units.measure.AngularVelocity> m_angularVelocity

    • m_kinematics

      protected edu.wpi.first.math.kinematics.SwerveDriveKinematics m_kinematics

    • m_odometry

      protected edu.wpi.first.math.estimator.SwerveDrivePoseEstimator m_odometry

    • m_modulePositions

      protected edu.wpi.first.math.kinematics.SwerveModulePosition[] m_modulePositions

    • m_moduleStates

      protected edu.wpi.first.math.kinematics.SwerveModuleState[] m_moduleStates

    • m_moduleLocations

      protected edu.wpi.first.math.geometry.Translation2d[] m_moduleLocations

    • m_odometryThread

      protected LegacySwerveDrivetrain.OdometryThread m_odometryThread

    • m_fieldRelativeOffset

      protected edu.wpi.first.math.geometry.Rotation2d m_fieldRelativeOffset

    • m_operatorForwardDirection

      protected edu.wpi.first.math.geometry.Rotation2d m_operatorForwardDirection

    • m_requestToApply

      protected LegacySwerveRequest m_requestToApply

    • m_requestParameters

      protected LegacySwerveRequest.LegacySwerveControlRequestParameters m_requestParameters

    • m_stateLock

      protected final ReadWriteLock m_stateLock

    • m_simDrive

      protected final LegacySimSwerveDrivetrain m_simDrive

    • m_telemetryFunction

      protected Consumer<LegacySwerveDrivetrain.LegacySwerveDriveState> m_telemetryFunction

    • m_cachedState

      protected final LegacySwerveDrivetrain.LegacySwerveDriveState m_cachedState

  • Constructor Details

    • LegacySwerveDrivetrain

      public LegacySwerveDrivetrain(LegacySwerveDrivetrainConstants driveTrainConstants, LegacySwerveModuleConstants... 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

    • LegacySwerveDrivetrain

      public LegacySwerveDrivetrain(LegacySwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, LegacySwerveModuleConstants... 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

    • LegacySwerveDrivetrain

      public LegacySwerveDrivetrain(LegacySwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> odometryStandardDeviation, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionStandardDeviation, LegacySwerveModuleConstants... 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()

    • getDaqThread

      public LegacySwerveDrivetrain.OdometryThread getDaqThread()
      Gets a reference to the data acquisition thread.
      Returns:
      DAQ thread

    • setControl

      public void setControl(LegacySwerveRequest 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(edu.wpi.first.math.geometry.Rotation2d fieldDirection)
      Takes the LegacySwerveRequest.ForwardReference.RedAlliance perpective direction and treats it as the forward direction for LegacySwerveRequest.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 LegacySwerveRequest.ForwardReference.RedAlliance perspective.

    • seedFieldRelative

      public void seedFieldRelative(edu.wpi.first.math.geometry.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 LegacySwerveModule 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 LegacySwerveDrivetrain.LegacySwerveDriveState getState()
      Gets the current state of the swerve drivetrain.
      Returns:
      Current state of the drivetrain

    • getRotation3d

      public edu.wpi.first.math.geometry.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(edu.wpi.first.math.geometry.Pose2d visionRobotPoseMeters, double timestampSeconds, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.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 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(edu.wpi.first.math.geometry.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 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(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.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<LegacySwerveDrivetrain.LegacySwerveDriveState> 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