Package frc.robot.subsystems.swerve

Class Swerve

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
frc.robot.subsystems.swerve.Swerve
All Implemented Interfaces:
Sendable, Subsystem
@NullMarked public final class Swerve extends SubsystemBase
Primary swerve drivetrain subsystem.

This subsystem owns and coordinates all components required to control and estimate the state of a swerve drive, including:

Threading model

Odometry-related sensor signals are updated on a dedicated background thread. Access to these signals and derived state is synchronized using a shared odometryLock to ensure consistency across the estimator and modules.

Pose estimation

Wheel encoder and gyro data are integrated at high rate to produce odometry updates, which are then fused with delayed vision measurements inside RobotState. The resulting pose estimate is the authoritative source of robot position for autonomous and field-relative driving.

Driving model

All drive commands ultimately resolve to robot-relative ChassisSpeeds. These speeds are passed through a SwerveRateLimiter before being discretized and converted to per-module states.

This class exposes convenience commands for robot-relative, field-relative, and user-relative driving, as well as trajectory-style pose targeting and characterization routines.

  • Field Details

    • modules

      public final SwerveModule[] modules

    • state

      public final RobotState state

    • customSkidLimit

      public double customSkidLimit

  • Constructor Details

  • Method Details

    • periodic

      public void periodic()

    • driveRobotRelative

      public Command driveRobotRelative(Supplier<ChassisSpeeds> driveSpeeds)
      Drives the robot using robot-relative chassis speeds.

      Supplied speeds are passed through the SwerveRateLimiter before being applied to the drivetrain.

      Parameters:
      driveSpeeds - supplier of robot-relative chassis speeds
      Returns:
      a command that drives the robot while scheduled

    • driveUserRelative

      public Command driveUserRelative(Supplier<ChassisSpeeds> driveSpeeds)
      Drives the robot using a user-defined field reference heading.

      The supplied field-relative speeds are transformed into robot-relative speeds using the user-controlled heading offset.

      Parameters:
      driveSpeeds - supplier of field-relative chassis speeds
      Returns:
      a command that drives the robot while scheduled

    • driveFieldRelative

      public Command driveFieldRelative(Supplier<ChassisSpeeds> driveSpeeds)
      Drives the robot using the estimated global field heading.

      The supplied field-relative speeds are transformed into robot-relative speeds using the current pose estimate from RobotState.

      Parameters:
      driveSpeeds - supplier of field-relative chassis speeds
      Returns:
      a command that drives the robot while scheduled

    • overridePose

      public Command overridePose(Supplier<Pose2d> newPose)
      Immediately sets the robot's pose to a new value, updating both the odometry estimator and the underlying simulation state (if any).

      This is useful when you need to forcibly override the robot's pose, for example during testing or at the start of autonomous in simulation.

      If you only want to update the odometry/estimator without affecting any simulation state, use RobotState.resetPose(Pose2d) instead.

      Parameters:
      newPose - a supplier that provides the new robot pose in field coordinates
      Returns:
      a command that applies the pose override once when scheduled

    • moveToPose

      public frc.robot.subsystems.swerve.util.MoveToPoseBuilder moveToPose()
      Creates a command builder for driving the robot to a target global pose.

      The generated command uses a holonomic controller and the current pose estimate to compute chassis speeds, which are rate-limited and applied to the drivetrain.

      Returns:
      a MoveToPoseBuilder for configuring pose targets

    • feedforwardCharacterization

      public Command feedforwardCharacterization()
      Creates a SysId routine for drivetrain feedforward characterization.

      This routine is used to identify kS and kV parameters for the swerve drive by applying controlled voltage steps and measuring resulting motion.

      Returns:
      a command that runs feedforward characterization

    • wheelRadiusCharacterization

      public Command wheelRadiusCharacterization()
      Creates a SysId routine for wheel radius characterization.

      This routine estimates the effective wheel radius by correlating commanded motion with measured yaw change.

      Returns:
      a command that runs wheel radius characterization

    • setFieldRelativeOffset

      public Command setFieldRelativeOffset(Supplier<Rotation2d> knownHeading)
      Sets a user-defined field-relative heading offset.

      This offset is used by user-relative driving modes to define "forward" independently of the robot's pose estimate.

      Parameters:
      knownHeading - supplier of the desired field heading
      Returns:
      a one-shot command that applies the offset

    • setFieldRelativeOffset

      public Command setFieldRelativeOffset()
      Sets a user-defined field-relative heading offset.

      This offset is used by user-relative driving modes to define "forward" independently of the robot's pose estimate.

      Returns:
      a one-shot command that sets the offset such that the current direction is "forward"

    • resetFieldRelativeOffsetBasedOnPose

      public Command resetFieldRelativeOffsetBasedOnPose()
      Sets a user-defined field-relative heading offset.

      This offset is used by user-relative driving modes to define "forward" independently of the robot's pose estimate.

      Returns:
      a one-shot command that sets the offset such that it agrees with the estimated pose (+180 degrees when on red alliance)

    • stop

      public Command stop()
      Creates a command that smoothly brings the drivetrain to a complete stop.

      This command commands zero desired chassis speeds and allows the SwerveRateLimiter to decelerate the robot within configured acceleration, tilt, and skid constraints rather than stopping abruptly.

      The command completes once the rate-limited translational speed falls below a small threshold, indicating the robot is effectively stationary. After completion, a final zero-speed command is issued to ensure all modules are explicitly commanded to stop.

      This is intended for use when transitioning between driving modes, autonomous steps, or before actions that require the robot to be fully settled.

      Returns:
      a command that decelerates and stops the drivetrain

    • emergencyStop

      public Command emergencyStop()
      Creates a command that immediately commands zero chassis speeds to the drivetrain.

      This method bypasses all rate limiting and deceleration constraints and directly commands the swerve modules to stop in a single control cycle. As a result, it may cause abrupt deceleration, increased state estimation error, and/or loss of traction depending on robot speed and surface conditions.

      In most situations, stop() should be preferred, as it brings the robot to rest in a controlled manner using the SwerveRateLimiter.

      This command is intended only for exceptional circumstances such as fault handling, disable transitions, or safety-critical interruptions where immediate cessation of motion is required.

      Returns:
      a command that immediately commands zero chassis speeds

    • limitSkidLimit

      public Command limitSkidLimit()

    • getUserRelativeHeading

      public Rotation2d getUserRelativeHeading()
      Returns the current user-relative heading used for driving.

      This heading is derived from the gyro yaw and a manually controlled field offset, independent of the pose estimator.

      Returns:
      user-relative field heading

    • closestTrench

      public Translation2d closestTrench()
      Identifies Closest Trench

    • distanceFromClosestTrench

      public double distanceFromClosestTrench()
      Returns the distance between the robot and the closest Trench zone.

    • inTrench

      public boolean inTrench()
      Returns true if the robot is in the Trench zone.

    • trenchSides

      public static final List<Translation2d> trenchSides()
      returns a list with all of the trench bounds

    • goalTrenchPosition

      public Translation2d goalTrenchPosition()
      Returns the goal location of the robot

    • moveToTrench

      public Command moveToTrench()
      Command that moves the robot to the trench without going through

    • moveThroughTrench

      public Command moveThroughTrench(Supplier<ChassisSpeeds> driveSpeeds)
      Command that moves the robot through the trench, constraining Y movement to stay in the trench.
      Parameters:
      driveSpeeds - supplier of field-relative chassis speeds
      Returns:
      a command that drives the robot while scheduled