Package com.frcteam3255.components.swerve

Class SN_SuperSwerve

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
com.frcteam3255.components.swerve.SN_SuperSwerve
All Implemented Interfaces:
edu.wpi.first.util.sendable.Sendable, edu.wpi.first.wpilibj2.command.Subsystem
public class SN_SuperSwerve extends edu.wpi.first.wpilibj2.command.SubsystemBase

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    com.pathplanner.lib.config.PIDConstants
    autoDrivePID
     
    HashMap<String,edu.wpi.first.wpilibj2.command.Command>
    autoEventMap
     
    BooleanSupplier
    autoFlipPaths
     
    com.pathplanner.lib.config.PIDConstants
    autoSteerPID
     
    com.ctre.phoenix6.configs.CANcoderConfiguration
    cancoderConfig
     
    com.ctre.phoenix6.configs.TalonFXConfiguration
    driveConfig
     
    com.pathplanner.lib.trajectory.PathPlannerTrajectory
    exampleAuto
     
    edu.wpi.first.wpilibj.smartdashboard.Field2d
    field
     
    boolean
    isFieldRelative
     
    double
    lastSimTime
     
    SN_SwerveModule[]
    modules
     
    com.ctre.phoenix6.hardware.Pigeon2
    pigeon
     
    double
    simAngle
     
    com.ctre.phoenix6.configs.TalonFXConfiguration
    steerConfig
     
    SN_SwerveConstants
    swerveConstants
     
    edu.wpi.first.math.kinematics.SwerveDriveKinematics
    swerveKinematics
     
    edu.wpi.first.math.estimator.SwerveDrivePoseEstimator
    swervePoseEstimator
     
    edu.wpi.first.math.controller.HolonomicDriveController
    teleopAutoDriveController
     
    double
    timeFromLastUpdate
     
    edu.wpi.first.units.measure.AngularVelocity
    turnSpeed
     

  • Constructor Summary

    Constructors
    Constructor
    Description
    SN_SuperSwerve(SN_SwerveConstants swerveConstants, SN_SwerveModule[] modules, double wheelbase, double trackWidth, String CANBusName, int pigeonCANId, double minimumSteerPercent, com.ctre.phoenix6.configs.TalonFXConfiguration driveConfig, com.ctre.phoenix6.configs.TalonFXConfiguration steerConfig, com.ctre.phoenix6.configs.CANcoderConfiguration cancoderConfig, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> stateStdDevs, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionStdDevs, com.pathplanner.lib.config.PIDConstants autoDrivePID, com.pathplanner.lib.config.PIDConstants autoSteerPID, edu.wpi.first.math.controller.HolonomicDriveController teleopAutoDriveController, edu.wpi.first.units.measure.AngularVelocity turnSpeed, com.pathplanner.lib.config.RobotConfig robotConfig, BooleanSupplier autoFlipPaths, boolean isSimulation)
    A superclass for a Swerve Drive.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d estimatedPose, double timestamp)
    Adds a vision measurement to the pose estimator.
    void
    autoAlign(edu.wpi.first.math.geometry.Pose2d desiredTarget, edu.wpi.first.math.kinematics.ChassisSpeeds manualVelocities, boolean isOpenLoop, boolean lockX, boolean lockY, boolean invertRotation)
    Automatically aligns the robot to a desired target pose while allowing for manual adjustments and additional control options.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    calculateVelocitiesFromInput(DoubleSupplier xAxisSupplier, DoubleSupplier yAxisSupplier, DoubleSupplier rotationAxisSupplier, BooleanSupplier slowMode, boolean isRed, double SLOW_MODE_MULTIPLIER, edu.wpi.first.units.measure.LinearVelocity REAL_DRIVE_SPEED, edu.wpi.first.units.measure.AngularVelocity TURN_SPEED)
    Calculates the chassis velocities based on joystick inputs and other parameters.
    void
    configure()
     
    void
    drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean isOpenLoop)
    Drive the drivetrain!
    void
    drive(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds, boolean isOpenLoop)
    Drive the drivetrain with pre-calculated ChassisSpeeds
    void
    driveAutonomous(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)
    Drive the drivetrain in autonomous.
    edu.wpi.first.math.kinematics.SwerveModuleState[]
    getActualModuleStates()
    Get the actual state (velocity, angle) of each module.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getChassisSpeeds()
    Returns the robot-relative chassis speeds.
    edu.wpi.first.math.geometry.Pose2d
    getClosestPoseByRotation(List<edu.wpi.first.math.geometry.Pose2d> poses)
    Calculate which pose from an array has the closest rotation to the robot's current pose.
    edu.wpi.first.math.kinematics.SwerveModuleState[]
    getDesiredModuleStates()
    Get the last desired states (velocity, angle) of each module.
    double
    getGyroRate()
     
    edu.wpi.first.math.kinematics.SwerveModulePosition[]
    getModulePositions()
    Get the position (distance, angle) of each module.
    edu.wpi.first.math.geometry.Pose2d
    getPose()
    Return the current estimated pose from the pose estimator.
    edu.wpi.first.math.geometry.Rotation2d
    getRotation()
    Return the current rotation of the robot using the pose estimator.
    edu.wpi.first.units.measure.AngularVelocity
    getVelocityToRotate(edu.wpi.first.math.geometry.Rotation2d desiredYaw)
    Returns the rotational velocity calculated with PID control to reach the given rotation.
    boolean
    isAtPosition(edu.wpi.first.math.geometry.Pose2d desiredPose2d, edu.wpi.first.units.measure.Distance tolerance)
     
    boolean
    isAtRotation(edu.wpi.first.math.geometry.Rotation2d desiredRotation, edu.wpi.first.units.measure.Angle tolerance)
     
    void
    neutralDriveOutputs()
    Sets all modules to neutral output
    void
    periodic()
     
    void
    resetModulesToAbsolute()
    Reset all of the steer motors's encoders to the absolute encoder values.
    void
    resetPoseToPose(edu.wpi.first.math.geometry.Pose2d pose)
    Reset the pose estimator's pose to a given pose.
    void
    rotationalAlign(edu.wpi.first.math.geometry.Pose2d desiredTarget, edu.wpi.first.math.kinematics.ChassisSpeeds velocities, boolean isOpenLoop)
    Aligns the robot's rotation to a desired target while maintaining the given translational velocities.
    void
    setFieldRelative()
    Sets the drive method to use field relative drive controls
    void
    setModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredModuleStates, boolean isOpenLoop)
    Set the states (velocity and position) of the modules.
    void
    setRobotRelative()
    Sets the drive method to use robot relative drive controls
    void
    updatePoseEstimator()
    Updates the pose estimator with the current robot uptime, the gyro yaw, and each swerve module position.
    void
    updateTimer()
    Must be run periodically in order to function properly!

    Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase

    addChild, getName, getSubsystem, initSendable, setName, setSubsystem

    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, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun

  • Field Details

    • modules

      public SN_SwerveModule[] modules

    • swervePoseEstimator

      public edu.wpi.first.math.estimator.SwerveDrivePoseEstimator swervePoseEstimator

    • swerveKinematics

      public edu.wpi.first.math.kinematics.SwerveDriveKinematics swerveKinematics

    • pigeon

      public com.ctre.phoenix6.hardware.Pigeon2 pigeon

    • isFieldRelative

      public boolean isFieldRelative

    • swerveConstants

      public SN_SwerveConstants swerveConstants

    • autoDrivePID

      public com.pathplanner.lib.config.PIDConstants autoDrivePID

    • autoSteerPID

      public com.pathplanner.lib.config.PIDConstants autoSteerPID

    • autoEventMap

      public HashMap<String,edu.wpi.first.wpilibj2.command.Command> autoEventMap

    • autoFlipPaths

      public BooleanSupplier autoFlipPaths

    • exampleAuto

      public com.pathplanner.lib.trajectory.PathPlannerTrajectory exampleAuto

    • simAngle

      public double simAngle

    • timeFromLastUpdate

      public double timeFromLastUpdate

    • lastSimTime

      public double lastSimTime

    • field

      public edu.wpi.first.wpilibj.smartdashboard.Field2d field

    • driveConfig

      public com.ctre.phoenix6.configs.TalonFXConfiguration driveConfig

    • steerConfig

      public com.ctre.phoenix6.configs.TalonFXConfiguration steerConfig

    • cancoderConfig

      public com.ctre.phoenix6.configs.CANcoderConfiguration cancoderConfig

    • teleopAutoDriveController

      public edu.wpi.first.math.controller.HolonomicDriveController teleopAutoDriveController

    • turnSpeed

      public edu.wpi.first.units.measure.AngularVelocity turnSpeed

  • Constructor Details

    • SN_SuperSwerve

      public SN_SuperSwerve(SN_SwerveConstants swerveConstants, SN_SwerveModule[] modules, double wheelbase, double trackWidth, String CANBusName, int pigeonCANId, double minimumSteerPercent, com.ctre.phoenix6.configs.TalonFXConfiguration driveConfig, com.ctre.phoenix6.configs.TalonFXConfiguration steerConfig, com.ctre.phoenix6.configs.CANcoderConfiguration cancoderConfig, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> stateStdDevs, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionStdDevs, com.pathplanner.lib.config.PIDConstants autoDrivePID, com.pathplanner.lib.config.PIDConstants autoSteerPID, edu.wpi.first.math.controller.HolonomicDriveController teleopAutoDriveController, edu.wpi.first.units.measure.AngularVelocity turnSpeed, com.pathplanner.lib.config.RobotConfig robotConfig, BooleanSupplier autoFlipPaths, boolean isSimulation)

      A superclass for a Swerve Drive. Your Drivetrain subsystem should extend from this class. In addition to the parameters required, it is highly recommended that you override SN_SwerveModule's driveConfiguration, steerConfiguration, and driveFeedForward in your configure method.

      View an example implementation here: https://github.com/FRCTeam3255/Standard_Swerve_Code

      Parameters:
      swerveConstants - The constants for all of your modules, such as gear ratio and max speed. You can create your own SN_SwerveConstants object or use a preset.
      modules - An array of SN_SwerveModules.
      wheelbase - Physically measured distance (center to center) between the Left and Right wheels in meters
      trackWidth - Physically measured distance (center to center) between the Front and Back wheels in meters
      CANBusName - The name of the CANBus that the Pigeon is on.
      pigeonCANId - The CAN id of the Pigeon.
      minimumSteerPercent - The minimum PercentOutput required to make the steer motor move
      driveConfig - The configuration for each drive motor. Make sure you set the inversion, neutral mode, and sensor to mechanism ratio!
      steerConfig - The configuration for each steer motor. Make sure you set the inversion, neutral mode, sensor to mechanism ratio, and continuous wrap!
      cancoderConfig - The configuration for each CANCoder. Make sure you set the sensor direction!
      stateStdDevs - Standard deviations of the pose estimate (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust your state estimate less.
      visionStdDevs - Standard deviations of vision pose measurements (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.
      autoDrivePID - The translational PID constants applied to the entire Drivetrain during autonomous in order to reach the correct pose
      autoSteerPID - The rotational PID constants applied to the entire Drivetrain during autonomous in order to reach the correct pose
      teleopAutoDriveController - The PID controller used to control rotational snapping and auto driving during Teleoperated.
      turnSpeed - The turning speed of the robot.
      robotConfig - The robot configuration used by PathPlanner.
      autoFlipPaths - Determines if paths should be flipped to the other side of the field. This will maintain a global blue alliance origin. Used for PathPlanner
      isSimulation - If your robot is running in Simulation. As of 2023, you can supply this with Robot.isSimulation();

  • Method Details

    • configure

      public void configure()

    • resetModulesToAbsolute

      public void resetModulesToAbsolute()
      Reset all of the steer motors's encoders to the absolute encoder values.

    • getModulePositions

      public edu.wpi.first.math.kinematics.SwerveModulePosition[] getModulePositions()
      Get the position (distance, angle) of each module.
      Returns:
      An Array of Swerve module positions (distance, angle)

    • getActualModuleStates

      public edu.wpi.first.math.kinematics.SwerveModuleState[] getActualModuleStates()
      Get the actual state (velocity, angle) of each module.
      Returns:
      An Array of Swerve module states (velocity, angle)

    • getDesiredModuleStates

      public edu.wpi.first.math.kinematics.SwerveModuleState[] getDesiredModuleStates()
      Get the last desired states (velocity, angle) of each module.
      Returns:
      An Array of Swerve module states (velocity, angle)

    • getChassisSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getChassisSpeeds()
      Returns the robot-relative chassis speeds.
      Returns:
      The robot-relative chassis speeds

    • setModuleStates

      public void setModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredModuleStates, boolean isOpenLoop)
      Set the states (velocity and position) of the modules.
      Parameters:
      desiredModuleStates - Desired states to set the modules to
      isOpenLoop - Are the modules being set based on open loop or closed loop control

    • drive

      public void drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean isOpenLoop)
      Drive the drivetrain!
      Parameters:
      translation - Desired translational velocity in meters per second
      rotation - Desired rotational velocity in radians per second
      isOpenLoop - Are the modules being set based on open loop or closed loop control

    • drive

      public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds, boolean isOpenLoop)
      Drive the drivetrain with pre-calculated ChassisSpeeds
      Parameters:
      chassisSpeeds - Desired ChassisSpeeds
      isOpenLoop - Are the modules being set based on open loop or closed loop control

    • driveAutonomous

      public void driveAutonomous(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)
      Drive the drivetrain in autonomous. Autonomous driving is always closed loop.
      Parameters:
      chassisSpeeds - Desired robot-relative chassis speeds

    • neutralDriveOutputs

      public void neutralDriveOutputs()
      Sets all modules to neutral output

    • setFieldRelative

      public void setFieldRelative()
      Sets the drive method to use field relative drive controls

    • setRobotRelative

      public void setRobotRelative()
      Sets the drive method to use robot relative drive controls

    • updatePoseEstimator

      public void updatePoseEstimator()
      Updates the pose estimator with the current robot uptime, the gyro yaw, and each swerve module position.

      This method MUST be called every loop (or else pose estimator breaks)

    • getPose

      public edu.wpi.first.math.geometry.Pose2d getPose()
      Return the current estimated pose from the pose estimator.
      Returns:
      The current estimated pose

    • getRotation

      public edu.wpi.first.math.geometry.Rotation2d getRotation()
      Return the current rotation of the robot using the pose estimator.
      Returns:
      The current estimated rotation

    • resetPoseToPose

      public void resetPoseToPose(edu.wpi.first.math.geometry.Pose2d pose)
      Reset the pose estimator's pose to a given pose.
      Parameters:
      pose - The pose you would like to reset the pose estimator to

    • getGyroRate

      public double getGyroRate()
      Returns:
      The current rate of rotation for the Pigeon 2. Units: Degrees per Second

    • addVisionMeasurement

      public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d estimatedPose, double timestamp)
      Adds a vision measurement to the pose estimator. This method does not need to be called periodically. The superclass of this class (SN_SuperSwerve) already updates the pose estimator every loop.
      Parameters:
      estimatedPose - The estimated pose measurement generated by vision
      timestamp - The timestamp of that pose estimate (not necessarily the current timestamp)

    • updateTimer

      public void updateTimer()

      Must be run periodically in order to function properly!

      Updates the values based on the current timestamp of the robot. Mainly required for simulation and discretize

    • getVelocityToRotate

      public edu.wpi.first.units.measure.AngularVelocity getVelocityToRotate(edu.wpi.first.math.geometry.Rotation2d desiredYaw)
      Returns the rotational velocity calculated with PID control to reach the given rotation. This must be called every loop until you reach the given rotation.
      Parameters:
      desiredYaw - The desired yaw to rotate to
      Returns:
      The desired velocity needed to rotate to that position.

    • rotationalAlign

      public void rotationalAlign(edu.wpi.first.math.geometry.Pose2d desiredTarget, edu.wpi.first.math.kinematics.ChassisSpeeds velocities, boolean isOpenLoop)
      Aligns the robot's rotation to a desired target while maintaining the given translational velocities. This method is typically used for rotational-only auto-alignment during swerve drive operations.
      Parameters:
      desiredTarget - The desired pose to align the robot's rotation to. Only the rotation component of the pose is used.
      velocities - The translational velocities (vx and vy) to maintain while aligning the rotation.
      isOpenLoop - If true, the drive system operates in open-loop mode; otherwise, it operates in closed-loop mode.

    • autoAlign

      public void autoAlign(edu.wpi.first.math.geometry.Pose2d desiredTarget, edu.wpi.first.math.kinematics.ChassisSpeeds manualVelocities, boolean isOpenLoop, boolean lockX, boolean lockY, boolean invertRotation)
      Automatically aligns the robot to a desired target pose while allowing for manual adjustments and additional control options.
      Parameters:
      desiredTarget - The target pose (position and orientation) to align to.
      manualVelocities - The manually controlled velocities to be applied, used when locking X or Y axes.
      isOpenLoop - Whether the drive system should operate in open-loop mode.
      lockX - If true, the X-axis velocity will be locked to the manual velocity.
      lockY - If true, the Y-axis velocity will be locked to the manual velocity.
      invertRotation - If true, the rotational velocity will be inverted.

    • isAtRotation

      public boolean isAtRotation(edu.wpi.first.math.geometry.Rotation2d desiredRotation, edu.wpi.first.units.measure.Angle tolerance)

    • isAtPosition

      public boolean isAtPosition(edu.wpi.first.math.geometry.Pose2d desiredPose2d, edu.wpi.first.units.measure.Distance tolerance)

    • getClosestPoseByRotation

      public edu.wpi.first.math.geometry.Pose2d getClosestPoseByRotation(List<edu.wpi.first.math.geometry.Pose2d> poses)
      Calculate which pose from an array has the closest rotation to the robot's current pose. If multiple poses have the same rotation, the last one in the list will be returned.
      Parameters:
      poses - The list of poses to check
      Returns:
      The last pose in the list with the closest rotation

    • calculateVelocitiesFromInput

      public edu.wpi.first.math.kinematics.ChassisSpeeds calculateVelocitiesFromInput(DoubleSupplier xAxisSupplier, DoubleSupplier yAxisSupplier, DoubleSupplier rotationAxisSupplier, BooleanSupplier slowMode, boolean isRed, double SLOW_MODE_MULTIPLIER, edu.wpi.first.units.measure.LinearVelocity REAL_DRIVE_SPEED, edu.wpi.first.units.measure.AngularVelocity TURN_SPEED)
      Calculates the chassis velocities based on joystick inputs and other parameters.
      Parameters:
      xAxisSupplier - A DoubleSupplier providing the x-axis input for forward/backward movement.
      yAxisSupplier - A DoubleSupplier providing the y-axis input for left/right movement.
      rotationAxisSupplier - A DoubleSupplier providing the rotation input for turning.
      slowMode - A BooleanSupplier indicating whether the slow mode is active.
      isRed - A boolean indicating if the robot is on the red alliance (reverses controls if true).
      SLOW_MODE_MULTIPLIER - A multiplier applied to velocities when slow mode is active.
      REAL_DRIVE_SPEED - The maximum linear velocity of the robot in meters per second.
      TURN_SPEED - The maximum angular velocity of the robot in radians per second.
      Returns:
      A ChassisSpeeds object containing the calculated x, y, and rotational velocities.

    • periodic

      public void periodic()