Class CommandSwerveDrivetrain

java.lang.Object
com.ctre.phoenix6.swerve.SwerveDrivetrain<com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.CANcoder>
frc.robot.generated.TunerConstants.TunerSwerveDrivetrain
frc.robot.generated.CommandSwerveDrivetrain
All Implemented Interfaces:
edu.wpi.first.wpilibj2.command.Subsystem, AutoCloseable

public class CommandSwerveDrivetrain extends TunerConstants.TunerSwerveDrivetrain implements edu.wpi.first.wpilibj2.command.Subsystem
Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily be used in command-based projects.
  • Nested Class Summary

    Nested classes/interfaces inherited from class com.ctre.phoenix6.swerve.SwerveDrivetrain

    com.ctre.phoenix6.swerve.SwerveDrivetrain.DeviceConstructor<DeviceT extends Object>, com.ctre.phoenix6.swerve.SwerveDrivetrain.OdometryThread, com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters, com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState
  • Field Summary

    Fields inherited from class com.ctre.phoenix6.swerve.SwerveDrivetrain

    kNumConfigAttempts, m_drivetrainId, m_jni, m_telemetryJNI
  • Constructor Summary

    Constructors
    Constructor
    Description
    CommandSwerveDrivetrain(com.ctre.phoenix6.swerve.SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, com.ctre.phoenix6.swerve.SwerveModuleConstants<?,?,?>... modules)
    Constructs a CTRE SwerveDrivetrain using the specified constants.
    CommandSwerveDrivetrain(com.ctre.phoenix6.swerve.SwerveDrivetrainConstants 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, com.ctre.phoenix6.swerve.SwerveModuleConstants<?,?,?>... modules)
    Constructs a CTRE SwerveDrivetrain using the specified constants.
    CommandSwerveDrivetrain(com.ctre.phoenix6.swerve.SwerveDrivetrainConstants drivetrainConstants, com.ctre.phoenix6.swerve.SwerveModuleConstants<?,?,?>... modules)
    Constructs a CTRE SwerveDrivetrain using the specified constants.
  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d visionRobotPoseMeters, double timestampSeconds)
    Adds a vision measurement to the Kalman Filter.
    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.
    edu.wpi.first.wpilibj2.command.Command
    applyRequest(Supplier<com.ctre.phoenix6.swerve.SwerveRequest> requestSupplier)
    Returns a command that applies the specified control request to this swerve drivetrain.
    void
     
    edu.wpi.first.wpilibj2.command.Command
    sysIdDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
    Runs the SysId Dynamic test in the given direction for the routine specified by sysIdRoutineToApply.
    edu.wpi.first.wpilibj2.command.Command
    sysIdQuasistatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
    Runs the SysId Quasistatic test in the given direction for the routine specified by sysIdRoutineToApply.

    Methods inherited from class com.ctre.phoenix6.swerve.SwerveDrivetrain

    close, configNeutralMode, configNeutralMode, getKinematics, getModule, getModuleLocations, getModules, getOdometryFrequency, getOdometryFrequencyMeasure, getOdometryThread, getOperatorForwardDirection, getPigeon2, getRotation3d, getState, getStateCopy, isOdometryValid, isOnCANFD, registerTelemetry, resetPose, resetRotation, resetTranslation, samplePoseAt, seedFieldCentric, seedFieldCentric, setControl, setOperatorPerspectiveForward, setStateStdDevs, setVisionMeasurementStdDevs, tareEverything, updateSimState

    Methods inherited from class java.lang.Object

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

    Methods inherited from interface edu.wpi.first.wpilibj2.command.Subsystem

    defer, getCurrentCommand, getDefaultCommand, getName, idle, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun
  • Constructor Details

    • CommandSwerveDrivetrain

      public CommandSwerveDrivetrain(com.ctre.phoenix6.swerve.SwerveDrivetrainConstants drivetrainConstants, com.ctre.phoenix6.swerve.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
    • CommandSwerveDrivetrain

      public CommandSwerveDrivetrain(com.ctre.phoenix6.swerve.SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, com.ctre.phoenix6.swerve.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
    • CommandSwerveDrivetrain

      public CommandSwerveDrivetrain(com.ctre.phoenix6.swerve.SwerveDrivetrainConstants 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, com.ctre.phoenix6.swerve.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

    • applyRequest

      public edu.wpi.first.wpilibj2.command.Command applyRequest(Supplier<com.ctre.phoenix6.swerve.SwerveRequest> requestSupplier)
      Returns a command that applies the specified control request to this swerve drivetrain.
      Parameters:
      requestSupplier - Function returning the request to apply
      Returns:
      Command to run
    • sysIdQuasistatic

      public edu.wpi.first.wpilibj2.command.Command sysIdQuasistatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
      Runs the SysId Quasistatic test in the given direction for the routine specified by sysIdRoutineToApply.
      Parameters:
      direction - Direction of the SysId Quasistatic test
      Returns:
      Command to run
    • sysIdDynamic

      public edu.wpi.first.wpilibj2.command.Command sysIdDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
      Runs the SysId Dynamic test in the given direction for the routine specified by sysIdRoutineToApply.
      Parameters:
      direction - Direction of the SysId Dynamic test
      Returns:
      Command to run
    • periodic

      public void periodic()
      Specified by:
      periodic in interface edu.wpi.first.wpilibj2.command.Subsystem
    • 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.
      Overrides:
      addVisionMeasurement in class com.ctre.phoenix6.swerve.SwerveDrivetrain<com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.CANcoder>
      Parameters:
      visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
      timestampSeconds - The timestamp of the vision measurement in seconds.
    • 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.

      Note that the vision measurement standard deviations passed into this method will continue to apply to future measurements until a subsequent call to SwerveDrivetrain.setVisionMeasurementStdDevs(Matrix) or this method.

      Overrides:
      addVisionMeasurement in class com.ctre.phoenix6.swerve.SwerveDrivetrain<com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.CANcoder>
      Parameters:
      visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
      timestampSeconds - The timestamp of the vision measurement in seconds.
      visionMeasurementStdDevs - Standard deviations of the vision pose measurement in the form [x, y, theta]ᵀ, with units in meters and radians.