Package frc.robot.subsystems.drivebase

Class DriveBaseSubsystem

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
frc.robot.shared.subsystems.AbstractSubsystem<DriveBaseSubsystemConfig>
frc.robot.subsystems.drivebase.DriveBaseSubsystem
All Implemented Interfaces:
edu.wpi.first.util.sendable.Sendable, edu.wpi.first.wpilibj2.command.Subsystem
public class DriveBaseSubsystem extends AbstractSubsystem<DriveBaseSubsystemConfig>
Provides a minimal API surface for commanding the robot drive base. It encapsulates the swerve hardware and exposes semantic operations for teleop driving and pose targeting so commands can focus on higher-level logic.

  • Field Summary

    Fields inherited from class frc.robot.shared.subsystems.AbstractSubsystem

    className, config, enabled, kDt, log, verbose

  • Constructor Summary

    Constructors
    Constructor
    Description
    DriveBaseSubsystem(DriveBaseSubsystemConfig config, Supplier<Boolean> isShootingSupplier)
    Creates the drive base subsystem and wires YAGSL hardware if enabled.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestampSeconds, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> standardDeviations)
    Forwards a vision measurement to the YAGSL internal pose estimator for Kalman-filter-based fusion.
    void
    driveFieldRelative(double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond)
    Drives the robot using field-relative chassis speeds.
    void
    driveFieldRelativeWithHeading(double vxMetersPerSecond, double vyMetersPerSecond, double targetHeadingRadians)
    Drives the robot field-relative with PID-controlled heading instead of manual omega.
    double
    getFieldFacingMarginRadians()
    Returns the configured field-facing margin in radians.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getFieldRelativeVelocity()
    Returns the robot's current velocity in the field coordinate frame.
    edu.wpi.first.math.geometry.Pose2d
    getFusedPose()
    Returns the vision-fused pose from YAGSL's internal pose estimator (odometry + vision corrections).
    edu.wpi.first.math.geometry.Pose2d
    getOdometryOnlyPose()
    Returns the raw odometry-only pose derived from wheel encoders and gyro.
    double
    getRotationToleranceRadians()
    Returns the configured rotation tolerance in radians for heading convergence checks.
    swervelib.SwerveDrive
    getSwerveDrive()
    Provides access to the configured swerve drive for command factories and testing helpers.
    double
    getYawRateRadiansPerSecond()
    Returns the robot's current yaw rate (rotational velocity around the vertical axis).
    double
    mapDriverOmega(double omegaAxis)
    Converts a pre-shaped driver omega axis into a scaled angular speed in radians per second.
    Supplier<Double>
    mapDriverOmegaSupplier(Supplier<Double> omegaAxisSupplier)
    Wraps mapDriverOmega(double) in a supplier so callers can hand the mapping function directly to commands.
    edu.wpi.first.math.geometry.Translation2d
    mapDriverTranslation(double forwardAxis, double leftAxis)
    Converts pre-shaped driver axes into scaled translation speeds in meters per second.
    Supplier<edu.wpi.first.math.geometry.Translation2d>
    mapDriverTranslationSupplier(Supplier<Double> forwardAxisSupplier, Supplier<Double> leftAxisSupplier)
    Wraps mapDriverTranslation(double, double) in a supplier so callers can hand the mapping function directly to commands.
    void
    periodic()
    Called every robot tick to update voltage smoothing and publish telemetry.
    void
    resetHeadingController()
    Resets the heading PID controller so accumulated integral error does not carry over between commands.
    void
    resetPose(edu.wpi.first.math.geometry.Pose2d pose)
    Resets odometry and gyro heading to the supplied pose.
    void
    stop()
    Stops all motion and locks the modules in their current orientation.

    Methods inherited from class frc.robot.shared.subsystems.AbstractSubsystem

    getConfig, isFMSAttached, isSimulation, isSubsystemDisabled, isVerbose, isVerboseLoggingEnabled, logDisabled, reportWarning

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

  • Constructor Details

    • DriveBaseSubsystem

      public DriveBaseSubsystem(DriveBaseSubsystemConfig config, Supplier<Boolean> isShootingSupplier)
      Creates the drive base subsystem and wires YAGSL hardware if enabled.
      Parameters:
      config - drive base configuration values and tunables
      isShootingSupplier - supplier that returns true when the robot is actively shooting; used for power management speed scaling

  • Method Details

    • periodic

      public void periodic()
      Called every robot tick to update voltage smoothing and publish telemetry.

    • getFusedPose

      public edu.wpi.first.math.geometry.Pose2d getFusedPose()
      Returns the vision-fused pose from YAGSL's internal pose estimator (odometry + vision corrections).

      This method is intended for drivebase-internal use: commands in this subsystem's factory, PathPlanner's AutoBuilder, and the heading PID controller. External subsystems and commands should use RobotPoseSubsystem.getEstimatedPose() for consistency, since RobotPoseSubsystem is the single source-of-truth pose supplier for cross-subsystem logic.

      Returns:
      current vision-fused pose in meters and radians

    • getOdometryOnlyPose

      public edu.wpi.first.math.geometry.Pose2d getOdometryOnlyPose()
      Returns the raw odometry-only pose derived from wheel encoders and gyro.

      This pose does not include vision corrections, so it will drift over time. Compare it with the fused estimate in AdvantageScope to see how much vision corrects for odometry error.

      Returns:
      raw odometry pose in meters and radians, without vision fusion

    • getYawRateRadiansPerSecond

      public double getYawRateRadiansPerSecond()
      Returns the robot's current yaw rate (rotational velocity around the vertical axis).

      This value comes from the measured chassis speeds reported by the swerve IO layer. Positive values indicate counter-clockwise rotation following WPILib conventions.

      Returns:
      yaw rate in radians per second

    • getFieldRelativeVelocity

      public edu.wpi.first.math.kinematics.ChassisSpeeds getFieldRelativeVelocity()
      Returns the robot's current velocity in the field coordinate frame.

      The returned speeds express how the robot moves relative to the field: vx along field +X (toward the opposing alliance wall), vy along field +Y (to the left when facing the opposing wall), and omega counter-clockwise. This is used by the shoot-on-the-move solver to compute how far the ball drifts during flight.

      Returns:
      field-relative chassis speeds in meters per second and radians per second

    • resetPose

      public void resetPose(edu.wpi.first.math.geometry.Pose2d pose)
      Resets odometry and gyro heading to the supplied pose.
      Parameters:
      pose - desired pose in meters and radians for the robot to assume immediately

    • addVisionMeasurement

      public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestampSeconds, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> standardDeviations)
      Forwards a vision measurement to the YAGSL internal pose estimator for Kalman-filter-based fusion.

      YAGSL wraps a SwerveDrivePoseEstimator that fuses encoder odometry with vision observations using latency compensation and configurable uncertainty weighting.

      Parameters:
      robotPose - vision-measured robot pose in meters and radians
      timestampSeconds - FPGA timestamp of the measurement in seconds
      standardDeviations - uncertainty in x (meters), y (meters), and rotation (radians)

    • driveFieldRelative

      public void driveFieldRelative(double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond)
      Drives the robot using field-relative chassis speeds.
      Parameters:
      vxMetersPerSecond - Field-forward velocity request.
      vyMetersPerSecond - Field-left velocity request.
      omegaRadiansPerSecond - Counter-clockwise rotational rate.

    • driveFieldRelativeWithHeading

      public void driveFieldRelativeWithHeading(double vxMetersPerSecond, double vyMetersPerSecond, double targetHeadingRadians)
      Drives the robot field-relative with PID-controlled heading instead of manual omega.

      Translation axes come from the driver stick. YAGSL's headingCalculate computes the angular velocity needed to reach the target heading by running the heading PID and scaling the output by the configured maximum angular velocity. This is the core API used by snap-to-heading commands and trench traversal.

      Parameters:
      vxMetersPerSecond - field-forward velocity in meters per second
      vyMetersPerSecond - field-left velocity in meters per second
      targetHeadingRadians - desired field-relative heading in radians (counter-clockwise positive)

    • resetHeadingController

      public void resetHeadingController()
      Resets the heading PID controller so accumulated integral error does not carry over between commands.

      Call this when a heading-lock command initializes to prevent the PID from producing a large initial output based on stale error history.

    • getRotationToleranceRadians

      public double getRotationToleranceRadians()
      Returns the configured rotation tolerance in radians for heading convergence checks.
      Returns:
      rotation tolerance in radians

    • getFieldFacingMarginRadians

      public double getFieldFacingMarginRadians()
      Returns the configured field-facing margin in radians. The robot is considered facing a field-aligned heading when it is within this many radians of that heading.
      Returns:
      field-facing margin in radians

    • mapDriverTranslation

      public edu.wpi.first.math.geometry.Translation2d mapDriverTranslation(double forwardAxis, double leftAxis)
      Converts pre-shaped driver axes into scaled translation speeds in meters per second. The input values should already have deadband and response curve processing applied by TriggerBindings.
      Parameters:
      forwardAxis - shaped forward stick value (+X on the field), in the range [-1, 1]
      leftAxis - shaped left stick value (+Y on the field), in the range [-1, 1]
      Returns:
      translation request in meters per second for field-relative driving

    • mapDriverTranslationSupplier

      public Supplier<edu.wpi.first.math.geometry.Translation2d> mapDriverTranslationSupplier(Supplier<Double> forwardAxisSupplier, Supplier<Double> leftAxisSupplier)
      Wraps mapDriverTranslation(double, double) in a supplier so callers can hand the mapping function directly to commands.
      Parameters:
      forwardAxisSupplier - supplier of the forward stick value (+X on the field)
      leftAxisSupplier - supplier of the left stick value (+Y on the field)
      Returns:
      supplier that produces a field-relative translation request in meters per second

    • mapDriverOmega

      public double mapDriverOmega(double omegaAxis)
      Converts a pre-shaped driver omega axis into a scaled angular speed in radians per second. The input value should already have deadband and response curve processing applied by TriggerBindings.

      Omega is the robot's rotational velocity around the vertical axis, measured in radians per second.

      Parameters:
      omegaAxis - shaped rotation stick value (positive for counter-clockwise), in the range [-1, 1]
      Returns:
      angular velocity request in radians per second

    • mapDriverOmegaSupplier

      public Supplier<Double> mapDriverOmegaSupplier(Supplier<Double> omegaAxisSupplier)
      Wraps mapDriverOmega(double) in a supplier so callers can hand the mapping function directly to commands.
      Parameters:
      omegaAxisSupplier - supplier of the rotation stick value (positive for counter-clockwise)
      Returns:
      supplier that produces a scaled angular velocity in radians per second

    • stop

      public void stop()
      Stops all motion and locks the modules in their current orientation.

    • getSwerveDrive

      public swervelib.SwerveDrive getSwerveDrive()
      Provides access to the configured swerve drive for command factories and testing helpers.
      Returns:
      configured SwerveDrive instance, or null if initialization failed or subsystem is disabled