Package frc.robot.subsystems.drive

Class Drive

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
frc.robot.subsystems.drive.Drive
All Implemented Interfaces:
Sendable, Subsystem, Monitored
public class Drive extends SubsystemBase implements Monitored
Subsystem for the robot's swerve drive.

Handles kinematics, odometry estimation (incorporating gyro, modules, and vision), and interfacing with PathPlanner for autonomous paths.

  • Constructor Details

    • Drive

      public Drive(GyroIO gyroIO, ModuleIO flModuleIO, ModuleIO frModuleIO, ModuleIO blModuleIO, ModuleIO brModuleIO)
      Creates a new Drive subsystem.
      Parameters:
      gyroIO - The abstraction layer for the gyroscope.
      flModuleIO - The abstraction layer for the front-left module.
      frModuleIO - The abstraction layer for the front-right module.
      blModuleIO - The abstraction layer for the back-left module.
      brModuleIO - The abstraction layer for the back-right module.

  • Method Details

    • headingAim

      public Command headingAim(Supplier<Rotation2d> targetSupplier)
      Command to maintain a specific heading using a supplier for the target angle.
      Parameters:
      targetSupplier - A supplier for the target heading (Rotation2d).
      Returns:
      A command that maintains the specified heading.

    • setIMUHighFreqConsumer

      public void setIMUHighFreqConsumer(Consumer<IMUState> callback)
      Returns the current pose of the robot on the field.
      Parameters:
      callback - A Consumer that accepts the current IMU state.

    • setHeadingOverrideSupplier

      public void setHeadingOverrideSupplier(Supplier<Rotation2d> supplier)
      Sets the supplier for the heading override.
      Parameters:
      supplier - A Supplier that provides the desired heading as a Rotation2d. If null, the robot will use the gyro heading for odometry and control.

    • periodic

      public void periodic()
      Specified by:
      periodic in interface Subsystem

    • applyRotationPriority

      public ChassisSpeeds applyRotationPriority(ChassisSpeeds speeds)
      Calculates translation scaling to ensure rotation requested is fully achieved.
      Parameters:
      speeds - The desired chassis speeds.
      Returns:
      The scaled chassis speeds with rotation priority.

    • runVelocity

      public void runVelocity(ChassisSpeeds speeds, Rotation2d targetHeading)
      The single entry point for all robot movement.
      Parameters:
      speeds - The desired linear/angular velocity.
      targetHeading - If non-null, the robot will ignore speeds.omega and calculate its own rotation with PRIORITY BUDGETING.

    • runVelocity

      public void runVelocity(ChassisSpeeds speeds)
      Main entry point for movement. This acts as a wrapper that modifies the incoming speeds based on the current Goal state.
      Parameters:
      speeds - Speeds relative to the robot's field-centric perspective.

    • runVelocityRaw

      public void runVelocityRaw(ChassisSpeeds speeds)
      Internal method to run velocity without re-discretizing
      Parameters:
      speeds - Speeds in meters/sec, relative to the robot's field-centric perspective.

    • runCharacterization

      public void runCharacterization(Voltage output)
      Runs the drive in a straight line with the specified drive output. Used for SysId.
      Parameters:
      output - The voltage to apply to the drive motors.

    • stop

      public void stop()
      Stops the drive.

    • stopWithX

      public void stopWithX()
      Stops the drive and turns the modules to an X arrangement to resist movement. The modules will return to their normal orientations the next time a nonzero velocity is requested.

    • sysIdQuasistatic

      public Command sysIdQuasistatic(SysIdRoutine.Direction direction)
      Returns a command to run a quasistatic test in the specified direction for characterization.
      Parameters:
      direction - The direction to run the test.
      Returns:
      A Command to execute the test.

    • sysIdDynamic

      public Command sysIdDynamic(SysIdRoutine.Direction direction)
      Returns a command to run a dynamic test in the specified direction for characterization.
      Parameters:
      direction - The direction to run the test.
      Returns:
      A Command to execute the test.

    • getChassisSpeeds

      public ChassisSpeeds getChassisSpeeds()
      Returns the measured chassis speeds of the robot based on module states.
      Returns:
      The current robot chassis speeds.

    • getFieldRelativeVelocity

      public Translation2d getFieldRelativeVelocity()
      Returns the field-relative velocity of the robot as a Translation2d (x = m/s forward on field, y = m/s left on field).
      Returns:
      The current field-relative linear velocity.

    • getWheelRadiusCharacterizationPositions

      public double[] getWheelRadiusCharacterizationPositions()
      Returns the position of each module in radians for characterization.
      Returns:
      An array of module positions in radians.

    • getFFCharacterizationVelocity

      public AngularVelocity getFFCharacterizationVelocity()
      Returns the average velocity of the modules for characterization.
      Returns:
      The average module velocity in radians per second.

    • getPose

      public Pose2d getPose()
      Returns the current odometry pose.
      Returns:
      The estimated pose of the robot on the field.

    • getYawVelocityRadPerSec

      public double getYawVelocityRadPerSec()
      Returns the current yaw (heading) velocity of the robot from the gyro.
      Returns:
      The current angular velocity around the yaw axis in radians per second.

    • getRotation

      public Rotation2d getRotation()
      Returns the current odometry rotation.
      Returns:
      The estimated rotation of the robot.

    • setPose

      public void setPose(Pose2d pose)
      Resets the current odometry pose to a specific position. This assumes the bot is flat on the ground and will break horrendously if not.
      Parameters:
      pose - The new pose to set the robot to.

    • addVisionMeasurement

      public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)
      Adds a new timestamped vision measurement to the pose estimator.
      Parameters:
      visionRobotPoseMeters - The pose reported by the vision system.
      timestampSeconds - The timestamp of the vision measurement in seconds.
      visionMeasurementStdDevs - The standard deviations of the vision measurement.

    • getMaxLinearVelocity

      public LinearVelocity getMaxLinearVelocity()
      Returns the maximum linear velocity.
      Returns:
      The maximum linear velocity.

    • getMaxAngularVelocity

      public AngularVelocity getMaxAngularVelocity()
      Returns the maximum angular velocity.
      Returns:
      The maximum angular velocity.

    • getKinematics

      public SwerveDriveKinematics getKinematics()
      Returns the swerve drive kinematics instance for this drivetrain.
      Returns:
      The SwerveDriveKinematics used by the drive subsystem.

    • isAlignedWithTarget

      public boolean isAlignedWithTarget(Rotation2d targetAngle)
      Checks if the robot is aligned within a certain tolerance to a target angle.
      Parameters:
      targetAngle - The angle to check against.
      Returns:
      True if within tolerance, false otherwise.

    • resetRotationController

      public void resetRotationController()
      Resets the rotation PID to prevent sudden jerks when taking over heading control.

    • calculateRotationFeedback

      public double calculateRotationFeedback(Rotation2d targetHeading)
      Calculates the closed-loop angular velocity required to reach the target heading.
      Parameters:
      targetHeading - The target heading as a Rotation2d.
      Returns:
      The required angular velocity.

    • isHealthy

      public boolean isHealthy()
      Returns whether or not the subsystem is healthy
      Specified by:
      isHealthy in interface Monitored
      Returns:
      True if the subsystem is healthy, false otherwise.

    • clearFaults

      public void clearFaults()
      Clears all faults and warnings.
      Specified by:
      clearFaults in interface Monitored