Package com.stuypulse.robot.subsystems.swerve

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>
com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain
com.stuypulse.robot.subsystems.swerve.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
    Modifier and Type
    Field
    Description
    private final edu.wpi.first.networktables.StructPublisher<edu.wpi.first.math.kinematics.ChassisSpeeds>
    chassisPublisher
     
    private final com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric
    fieldCentricRequest
     
    private static final CommandSwerveDrivetrain
    instance
     
    private static final double
    kSimLoopPeriod
     
    private double
    m_lastSimTime
     
    private final com.ctre.phoenix6.swerve.SwerveRequest.SysIdSwerveTranslation
    m_moduleTranslationCharacterization
     
    private final com.ctre.phoenix6.swerve.SwerveRequest.SysIdSwerveRotation
    m_rotationCharacterization
     
    private edu.wpi.first.wpilibj.Notifier
    m_simNotifier
     
    private final com.ctre.phoenix6.swerve.SwerveRequest.SysIdSwerveSteerGains
    m_steerCharacterization
     
    private final edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
    m_sysIdRoutineChassisTranslation
     
    private final edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
    m_sysIdRoutineModuleTranslation
     
    private final edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
    m_sysIdRoutineRotation
     
    private final edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
    m_sysIdRoutineSteer
     
    private edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
    m_sysIdRoutineToApply
     
    private MapleSimSwerveDrivetrain
    mapleSimSwerveDrivetrain
     
    private final edu.wpi.first.networktables.StructArrayPublisher<edu.wpi.first.math.kinematics.SwerveModuleState>
    modulePublisher
     
    private final edu.wpi.first.networktables.StructPublisher<edu.wpi.first.math.geometry.Pose2d>
    posePublisher
     
    private final com.ctre.phoenix6.swerve.SwerveRequest.RobotCentric
    robotCentricRequest
     

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

    kNumConfigAttempts, m_drivetrainId, m_jni, m_telemetryJNI

  • Constructor Summary

    Constructors
    Modifier
    Constructor
    Description
    private
    CommandSwerveDrivetrain(com.ctre.phoenix6.swerve.SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, com.ctre.phoenix6.swerve.SwerveModuleConstants<?,?,?>... modules)
    Constructs a CTRE SwerveDrivetrain using the specified constants.
    private
    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.
    protected
    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.
    void
    configureAutoBuilder()
     
    void
    drive(edu.wpi.first.math.geometry.Translation2d velocity, double rotation)
     
    edu.wpi.first.wpilibj2.command.Command
    followPathCommand(com.pathplanner.lib.path.PathPlannerPath path)
     
    edu.wpi.first.wpilibj2.command.Command
    followPathCommand(String pathName)
     
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getChassisSpeeds()
     
    com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric
    getFieldCentricSwerveRequest()
     
    edu.wpi.first.math.geometry.Translation2d
    getFieldRelativeSpeeds()
     
    static CommandSwerveDrivetrain
    getInstance()
     
    org.ironmaple.simulation.drivesims.SwerveDriveSimulation
    getMapleSimDrive()
     
    edu.wpi.first.math.kinematics.SwerveModuleState[]
    getModuleStates()
     
    edu.wpi.first.math.geometry.Pose2d
    getPose()
     
    com.ctre.phoenix6.swerve.SwerveRequest.RobotCentric
    getRobotCentricSwerveRequest()
     
    edu.wpi.first.math.geometry.Pose2d
    getShooterPose()
     
    boolean
    isAlignedToTarget(edu.wpi.first.math.geometry.Pose2d target)
     
    void
    periodic()
     
    void
    resetPose(edu.wpi.first.math.geometry.Pose2d pose)
     
    private void
    setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds robotSpeeds)
     
    void
    setControl(com.ctre.phoenix6.swerve.SwerveRequest request)
     
    private void
    startSimThread()
     
    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 m_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 m_sysIdRoutineToApply.
    edu.wpi.first.wpilibj2.command.Command
    sysidRotationDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
     
    edu.wpi.first.wpilibj2.command.Command
    sysidRotationQuasiStatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
     
    edu.wpi.first.wpilibj2.command.Command
    sysidSteerDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
     
    edu.wpi.first.wpilibj2.command.Command
    sysidSteerQuasistatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)
     

    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, optimizeBusUtilization, optimizeBusUtilization, optimizeBusUtilization, registerTelemetry, resetRotation, resetTranslation, samplePoseAt, seedFieldCentric, seedFieldCentric, 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

  • Field Details

  • Constructor Details

    • CommandSwerveDrivetrain

      protected 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

      private 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

      private 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

    • getInstance

      public static CommandSwerveDrivetrain getInstance()

    • getFieldCentricSwerveRequest

      public com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric getFieldCentricSwerveRequest()

    • getRobotCentricSwerveRequest

      public com.ctre.phoenix6.swerve.SwerveRequest.RobotCentric getRobotCentricSwerveRequest()

    • setControl

      public void setControl(com.ctre.phoenix6.swerve.SwerveRequest request)
      Overrides:
      setControl in class com.ctre.phoenix6.swerve.SwerveDrivetrain<com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.CANcoder>

    • 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 m_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 m_sysIdRoutineToApply.
      Parameters:
      direction - Direction of the SysId Dynamic test
      Returns:
      Command to run

    • sysidRotationDynamic

      public edu.wpi.first.wpilibj2.command.Command sysidRotationDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)

    • sysidRotationQuasiStatic

      public edu.wpi.first.wpilibj2.command.Command sysidRotationQuasiStatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)

    • sysidSteerDynamic

      public edu.wpi.first.wpilibj2.command.Command sysidSteerDynamic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)

    • sysidSteerQuasistatic

      public edu.wpi.first.wpilibj2.command.Command sysidSteerQuasistatic(edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction direction)

    • startSimThread

      private void startSimThread()

    • getMapleSimDrive

      public org.ironmaple.simulation.drivesims.SwerveDriveSimulation getMapleSimDrive()

    • 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(Matrgix) 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.

    • getPose

      public edu.wpi.first.math.geometry.Pose2d getPose()

    • isAlignedToTarget

      public boolean isAlignedToTarget(edu.wpi.first.math.geometry.Pose2d target)

    • getShooterPose

      public edu.wpi.first.math.geometry.Pose2d getShooterPose()

    • resetPose

      public void resetPose(edu.wpi.first.math.geometry.Pose2d pose)
      Overrides:
      resetPose in class com.ctre.phoenix6.swerve.SwerveDrivetrain<com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.TalonFX,com.ctre.phoenix6.hardware.CANcoder>

    • configureAutoBuilder

      public void configureAutoBuilder()

    • followPathCommand

      public edu.wpi.first.wpilibj2.command.Command followPathCommand(String pathName)

    • followPathCommand

      public edu.wpi.first.wpilibj2.command.Command followPathCommand(com.pathplanner.lib.path.PathPlannerPath path)

    • getModuleStates

      public edu.wpi.first.math.kinematics.SwerveModuleState[] getModuleStates()

    • getChassisSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getChassisSpeeds()

    • getFieldRelativeSpeeds

      public edu.wpi.first.math.geometry.Translation2d getFieldRelativeSpeeds()

    • setChassisSpeeds

      private void setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds robotSpeeds)

    • drive

      public void drive(edu.wpi.first.math.geometry.Translation2d velocity, double rotation)

    • periodic

      public void periodic()
      Specified by:
      periodic in interface edu.wpi.first.wpilibj2.command.Subsystem