Class SwerveDrive

java.lang.Object
yams.core.mechanisms.swerve.SwerveDrive
Direct Known Subclasses:
SwerveDrive, SwerveDrive
public class SwerveDrive extends Object
Swerve Drive mechanism.

This core class holds swerve state, odometry, and physics simulation only. Command factories such as drive() live on SwerveDrive, which extends this class. See that class's Javadoc for a full usage example.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    protected SwerveDriveTelemetry
    m_swerveTelemetry
    Telemetry and live tuning of the drive.

  • Constructor Summary

    Constructors
    Modifier
    Constructor
    Description
    protected
    SwerveDrive(SwerveDriveConfig<?> config)
    Create a SwerveDrive.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addVisionMeasurement(org.wpilib.math.geometry.Pose2d robotPose, double timestamp)
    Add a vision measurement to the SwerveDrivePoseEstimator and update the gyro reading with the given timestamp of the vision measurement.
    void
    addVisionMeasurement(org.wpilib.math.geometry.Pose2d robotPose, double timestamp, org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N1> visionMeasurementStdDevs)
    Add a vision measurement to the SwerveDrivePoseEstimator and update the gyro reading with the given timestamp of the vision measurement.
    void
    applyTuningValues()
    Apply the live-tuned values (from NetworkTables) to this SwerveDrive's drive-to-pose controllers.
    org.wpilib.math.kinematics.ChassisVelocities
    driveToPoseSetpoint(org.wpilib.math.geometry.Pose2d targetPose)
    Drive to the target pose, primarily for use in Live Tuning, could also be used for setpoint commands.
    org.wpilib.math.kinematics.ChassisVelocities
    driveToPoseSetpoint(org.wpilib.math.geometry.Pose2d targetPose, org.wpilib.math.geometry.Pose2d currentPose, org.wpilib.math.geometry.Rotation2d heading)
    Drive to the target pose from a given pose, e.g.
    org.wpilib.units.measure.Angle
    getAngleDifferenceFromPose(org.wpilib.math.geometry.Pose2d pose)
    Get the angle difference between the robot's current pose and the given pose.
    SwerveDriveConfig<?>
    getConfig()
    Get the SwerveDriveConfig of the drive.
    org.wpilib.math.kinematics.ChassisVelocities
    getDesiredChassisSpeeds()
    Get the last-commanded desired robot-relative ChassisVelocities of the drive.
    org.wpilib.math.kinematics.SwerveModuleVelocity[]
    getDesiredModuleStates()
    Get the last-commanded desired SwerveModuleVelocitys of the drive.
    org.wpilib.units.measure.Distance
    getDistanceFromPose(org.wpilib.math.geometry.Pose2d pose)
    Get the Distance from the given pose to the robot.
    org.wpilib.smartdashboard.Field2d
    getField2d()
    Get the Field2d used to display the robot's pose, so callers (e.g.
    org.wpilib.math.kinematics.ChassisVelocities
    getFieldRelativeSpeed()
    Get the field relative speed of the drive.
    org.wpilib.math.geometry.Rotation3d
    getGyroRotation3d()
    Get the robot's attitude from the gyro (see SwerveDriveConfig.getGyroRotation3d()): its yaw is the robot's heading.
    org.wpilib.math.kinematics.SwerveDriveKinematics
    getKinematics()
    Create the SwerveDriveKinematics so you can recreate a new SwerveDrivePoseEstimator.
    Optional<SwerveModule>
    getModule(String moduleName)
    Get a module by its name.
    org.wpilib.math.kinematics.SwerveModulePosition[]
    getModulePositions()
    Get the SwerveModulePosition of the modules.
    SwerveModule[]
    getModules()
    Get all SwerveModules for this SwerveDrive.
    org.wpilib.math.kinematics.SwerveModuleVelocity[]
    getModuleStates()
    Get the SwerveModuleVelocity of the modules.
    String
    getName()
    Get the name of the drive.
    org.wpilib.math.geometry.Pose2d
    getPose()
    Gets the measured pose (position and rotation) of the robot, as reported by odometry.
    org.wpilib.math.kinematics.ChassisVelocities
    getRobotRelativeChassisSpeedsFromState(org.wpilib.math.kinematics.SwerveModuleVelocity[] states)
    Get the ChassisVelocities based off the SwerveModuleVelocitys.
    org.wpilib.math.kinematics.ChassisVelocities
    getRobotRelativeSpeed()
    Get the robot relative speed of the drive.
    org.wpilib.math.geometry.Pose2d
    getSimPose()
    Get the simulated pose of the robot, assuming the SwerveModuleVelocitys commanded to the modules are met perfectly.
    org.wpilib.math.kinematics.SwerveModuleVelocity[]
    getStateFromRobotRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities robotRelativeChassisSpeeds)
    Get the SwerveModuleVelocitys of the swerve drive given a robot relative chassis speed.
    boolean
    isNear(org.wpilib.math.geometry.Pose2d pose, org.wpilib.units.measure.Distance translationTolerance, org.wpilib.units.measure.Angle rotationTolerance)
    Whether the robot is within the given tolerances of a pose.
    void
    lockPose()
    Point all modules toward the robot center, thus making the robot very difficult to move.
    void
    resetOdometry(org.wpilib.math.geometry.Pose2d pose)
    Resets odometry to the given pose, and sets the gyro to read the pose's heading.
    void
    resetRotationPID()
    Resets the auto-align rotational PID controller, if one is configured.
    void
    resetTranslationPID()
    Resets the auto-align translation PID controller, if one is configured.
    void
    setAzimuthAngles(org.wpilib.math.geometry.Rotation2d angle)
    Turn every module to the given angle without driving, as for tuning the azimuth motors.
    void
    setFieldRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities fieldRelativeChassisSpeeds)
    Set field relative chassis speeds.
    void
    setRobotRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities robotRelativeChassisSpeeds)
    Set robot relative chassis speeds.
    void
    setRobotRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities robotRelativeChassisSpeeds, org.wpilib.units.measure.Force[] feedforwardForces)
    Set robot relative chassis speeds, with an additional drive wheel feedforward Force applied per module, e.g.
    void
    setRotationPID(org.wpilib.math.controller.PIDController controller)
    Set the auto-align rotational PID controller.
    void
    setSwerveModuleStates(org.wpilib.math.kinematics.SwerveModuleVelocity[] states)
    Set the SwerveModuleVelocitys of the swerve drive directly.
    void
    setSwerveModuleStates(org.wpilib.math.kinematics.SwerveModuleVelocity[] states, org.wpilib.units.measure.Force[] feedforwardForces)
    Set the SwerveModuleVelocitys of the swerve drive directly, with an additional drive wheel feedforward Force applied per module, e.g.
    void
    setTranslationPID(org.wpilib.math.controller.PIDController controller)
    Sets the auto-align translation PID controller.
    void
    setVisionMeasurementStdDevs(org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N1> visionMeasurementStdDevs)
    Sets the pose estimator's trust of global measurements.
    void
    simIterate()
    Simulate the drive, updating the gyroscope based off of module states.
    void
    startDriveToPoseTuning()
    Reset the auto-align PID controllers and pull the live-tuned values (from NetworkTables) into this SwerveDrive.
    void
    stop()
    Stop the drive by commanding zero chassis speeds.
    void
    updateTelemetry()
    Update the telemetry and SwerveDrivePoseEstimator of the drive.
    void
    zeroGyro()
    Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0 (red alliance station).

    Methods inherited from class Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Field Details

    • m_swerveTelemetry

      protected SwerveDriveTelemetry m_swerveTelemetry
      Telemetry and live tuning of the drive.

  • Constructor Details

  • Method Details

    • startDriveToPoseTuning

      public void startDriveToPoseTuning()
      Reset the auto-align PID controllers and pull the live-tuned values (from NetworkTables) into this SwerveDrive. Intended to be wired into a periodic/scheduled callback by the command layer, e.g. via yams.commands2.swerve.SwerveDrive's drive-to-pose tuning command.

    • applyTuningValues

      public void applyTuningValues()
      Apply the live-tuned values (from NetworkTables) to this SwerveDrive's drive-to-pose controllers. Intended to be wired into a periodic/scheduled callback by the command layer.
      Throws:
      IllegalStateException - if the auto-align tuning toggle is enabled and a gyro angular velocity scale factor is configured with SwerveDriveConfig.withGyroAngularVelocityScaleFactor(double) but no gyro supplier was set with SwerveDriveConfig.withGyro(java.util.function.Supplier).

    • getGyroRotation3d

      public org.wpilib.math.geometry.Rotation3d getGyroRotation3d()
      Get the robot's attitude from the gyro (see SwerveDriveConfig.getGyroRotation3d()): its yaw is the robot's heading. In simulation, the simulated heading with no tilt.
      Returns:
      The robot's attitude.

    • getSimPose

      public org.wpilib.math.geometry.Pose2d getSimPose()
      Get the simulated pose of the robot, assuming the SwerveModuleVelocitys commanded to the modules are met perfectly. Useful for feeding a simulated vision system with ground-truth poses under perfect-world conditions.
      Returns:
      Simulated Pose2d of the robot. Only updated in simulation by simIterate(); on a real robot this remains the configured starting pose.

    • lockPose

      public void lockPose()
      Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep the current pose.
      Implementation Note:
      Not compatible with AdvantageKit

    • setAzimuthAngles

      public void setAzimuthAngles(org.wpilib.math.geometry.Rotation2d angle)
      Turn every module to the given angle without driving, as for tuning the azimuth motors. The velocity deadband does not hold the wheels where they are (see SwerveModule.setAzimuthAngle(Rotation2d)).
      Parameters:
      angle - Angle to turn every module to.

    • setSwerveModuleStates

      public void setSwerveModuleStates(org.wpilib.math.kinematics.SwerveModuleVelocity[] states)
      Set the SwerveModuleVelocitys of the swerve drive directly.
      Parameters:
      states - SwerveModuleVelocitys to use, must be the same count as the swerve drive is configured order is Clockwise from FL.
      Implementation Note:
      Not compatible with AdvantageKit if MapleSim is defined.

    • setSwerveModuleStates

      public void setSwerveModuleStates(org.wpilib.math.kinematics.SwerveModuleVelocity[] states, org.wpilib.units.measure.Force[] feedforwardForces)
      Set the SwerveModuleVelocitys of the swerve drive directly, with an additional drive wheel feedforward Force applied per module, e.g. from a PathPlanner set-point generator.
      Parameters:
      states - SwerveModuleVelocitys to use, must be the same count as the swerve drive is configured order is Clockwise from FL.
      feedforwardForces - Feedforward Forces to apply, one per module in the same FL, FR, BL, BR order as states.
      Implementation Note:
      Not compatible with AdvantageKit if MapleSim is defined.

    • getStateFromRobotRelativeChassisSpeeds

      public org.wpilib.math.kinematics.SwerveModuleVelocity[] getStateFromRobotRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities robotRelativeChassisSpeeds)
      Get the SwerveModuleVelocitys of the swerve drive given a robot relative chassis speed. When a maximum module speed is configured with SwerveDriveConfig.withMaximumModuleSpeed(LinearVelocity), the module speeds are scaled down together so none exceeds it, also respecting SwerveDriveConfig.withMaximumChassisSpeed(LinearVelocity, AngularVelocity) if set, which keeps the direction of travel and rotation intact.
      Parameters:
      robotRelativeChassisSpeeds - Robot relative ChassisVelocities.
      Returns:
      SwerveModuleVelocitys of the swerve drive.
      Throws:
      IllegalStateException - if a gyro angular velocity scale factor is configured with SwerveDriveConfig.withGyroAngularVelocityScaleFactor(double) but no gyro supplier was set with SwerveDriveConfig.withGyro(java.util.function.Supplier).

    • getRobotRelativeChassisSpeedsFromState

      public org.wpilib.math.kinematics.ChassisVelocities getRobotRelativeChassisSpeedsFromState(org.wpilib.math.kinematics.SwerveModuleVelocity[] states)
      Get the ChassisVelocities based off the SwerveModuleVelocitys.
      Parameters:
      states - SwerveModuleVelocitys to use, must be the same count as the swerve drive is.
      Returns:
      ChassisVelocities based off the SwerveModuleVelocitys.

    • setRobotRelativeChassisSpeeds

      public void setRobotRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities robotRelativeChassisSpeeds)
      Set robot relative chassis speeds.
      Parameters:
      robotRelativeChassisSpeeds - Robot relative chassis speeds.
      Throws:
      IllegalStateException - if a gyro angular velocity scale factor is configured with SwerveDriveConfig.withGyroAngularVelocityScaleFactor(double) but no gyro supplier was set with SwerveDriveConfig.withGyro(java.util.function.Supplier).

    • setRobotRelativeChassisSpeeds

      public void setRobotRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities robotRelativeChassisSpeeds, org.wpilib.units.measure.Force[] feedforwardForces)
      Set robot relative chassis speeds, with an additional drive wheel feedforward Force applied per module, e.g. from a PathPlanner set-point generator.
      Parameters:
      robotRelativeChassisSpeeds - Robot relative chassis speeds.
      feedforwardForces - Feedforward Forces to apply, one per module in FL, FR, BL, BR order. Pass an empty array to apply no feedforward.
      Throws:
      IllegalArgumentException - if feedforwardForces is not empty and its length does not match the number of modules.
      IllegalStateException - if a gyro angular velocity scale factor is configured with SwerveDriveConfig.withGyroAngularVelocityScaleFactor(double) but no gyro supplier was set with SwerveDriveConfig.withGyro(java.util.function.Supplier).

    • setFieldRelativeChassisSpeeds

      public void setFieldRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities fieldRelativeChassisSpeeds)
      Set field relative chassis speeds.
      Parameters:
      fieldRelativeChassisSpeeds - Field relative chassis speeds.
      Throws:
      IllegalStateException - if a gyro angular velocity scale factor is configured with SwerveDriveConfig.withGyroAngularVelocityScaleFactor(double) but no gyro supplier was set with SwerveDriveConfig.withGyro(java.util.function.Supplier).

    • getPose

      public org.wpilib.math.geometry.Pose2d getPose()
      Gets the measured pose (position and rotation) of the robot, as reported by odometry.
      Returns:
      The robot's pose

    • getKinematics

      public org.wpilib.math.kinematics.SwerveDriveKinematics getKinematics()
      Create the SwerveDriveKinematics so you can recreate a new SwerveDrivePoseEstimator.
      Returns:
      SwerveDriveKinematics

    • zeroGyro

      public void zeroGyro()
      Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0 (red alliance station).
      Implementation Note:
      Not compatible with AdvantageKit

    • getName

      public String getName()
      Get the name of the drive.
      Returns:
      Name of the drive.

    • resetOdometry

      public void resetOdometry(org.wpilib.math.geometry.Pose2d pose)
      Resets odometry to the given pose, and sets the gyro to read the pose's heading. Field relative driving and heading control use the gyro, so keeping it aligned with the pose makes them agree with the reset pose. Module positions do not need to be reset when calling this method.
      Parameters:
      pose - The pose to set the odometry to. Field relative, blue-origin where 0deg is facing towards RED alliance.

    • resetRotationPID

      public void resetRotationPID()
      Resets the auto-align rotational PID controller, if one is configured.

    • resetTranslationPID

      public void resetTranslationPID()
      Resets the auto-align translation PID controller, if one is configured.

    • setRotationPID

      public void setRotationPID(org.wpilib.math.controller.PIDController controller)
      Set the auto-align rotational PID controller.
      Parameters:
      controller - PIDController to use, units given is in Radians.

    • setTranslationPID

      public void setTranslationPID(org.wpilib.math.controller.PIDController controller)
      Sets the auto-align translation PID controller.
      Parameters:
      controller - PIDController to reset, Units given is in Meters.

    • getDistanceFromPose

      public org.wpilib.units.measure.Distance getDistanceFromPose(org.wpilib.math.geometry.Pose2d pose)
      Get the Distance from the given pose to the robot.
      Parameters:
      pose - Pose2d to get the distance from.
      Returns:
      Distance from the given pose to the robot.

    • getAngleDifferenceFromPose

      public org.wpilib.units.measure.Angle getAngleDifferenceFromPose(org.wpilib.math.geometry.Pose2d pose)
      Get the angle difference between the robot's current pose and the given pose.
      Parameters:
      pose - Pose2d to get the angle difference from.
      Returns:
      Angle difference between the robot's current pose and the given pose.

    • isNear

      public boolean isNear(org.wpilib.math.geometry.Pose2d pose, org.wpilib.units.measure.Distance translationTolerance, org.wpilib.units.measure.Angle rotationTolerance)
      Whether the robot is within the given tolerances of a pose.
      Parameters:
      pose - Pose2d to compare against.
      translationTolerance - Maximum distance from the pose.
      rotationTolerance - Maximum heading error from the pose.
      Returns:
      True if the robot is within both tolerances of the pose.

    • stop

      public void stop()
      Stop the drive by commanding zero chassis speeds.
      Throws:
      IllegalStateException - if a gyro angular velocity scale factor is configured but no gyro supplier was set with withGyro(Supplier).

    • driveToPoseSetpoint

      public org.wpilib.math.kinematics.ChassisVelocities driveToPoseSetpoint(org.wpilib.math.geometry.Pose2d targetPose)
      Drive to the target pose, primarily for use in Live Tuning, could also be used for setpoint commands.
      Parameters:
      targetPose - Pose to drive towards.
      Returns:
      robot-relative ChassisVelocities to drive the robot to the given pose.
      Throws:
      SwerveDriveConfigurationException - if the translation PID controller is not configured.
      SwerveDriveConfigurationException - if the rotation PID controller is not configured.
      Implementation Note:
      Remember to call resetRotationPID() and resetTranslationPID() before calling this method in a loop.

    • driveToPoseSetpoint

      public org.wpilib.math.kinematics.ChassisVelocities driveToPoseSetpoint(org.wpilib.math.geometry.Pose2d targetPose, org.wpilib.math.geometry.Pose2d currentPose, org.wpilib.math.geometry.Rotation2d heading)
      Drive to the target pose from a given pose, e.g. a pose replayed from a log. The translation speed is the translation PID output toward the target, proportional to the distance for a P controller.
      Parameters:
      targetPose - Pose to drive towards.
      currentPose - Pose of the robot.
      heading - Heading of the robot, used to convert the result to robot-relative speeds.
      Returns:
      robot-relative ChassisVelocities to drive the robot to the given pose.
      Throws:
      SwerveDriveConfigurationException - if the translation PID controller is not configured.
      SwerveDriveConfigurationException - if the rotation PID controller is not configured.
      Implementation Note:
      Remember to call resetRotationPID() and resetTranslationPID() before calling this method in a loop.

    • addVisionMeasurement

      public void addVisionMeasurement(org.wpilib.math.geometry.Pose2d robotPose, double timestamp, org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N1> visionMeasurementStdDevs)
      Add a vision measurement to the SwerveDrivePoseEstimator and update the gyro reading with the given timestamp of the vision measurement.
      Parameters:
      robotPose - Robot Pose2d as measured by vision.
      timestamp - Timestamp the measurement was taken as time since startup, should be taken from Timer.getTimestamp() or similar sources.
      visionMeasurementStdDevs - Vision measurement standard deviation that will be sent to the SwerveDrivePoseEstimator.The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more points and fit a line to it with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get * vision accurate to inches instead of feet.

    • setVisionMeasurementStdDevs

      public void setVisionMeasurementStdDevs(org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N1> visionMeasurementStdDevs)
      Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases.
      Parameters:
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta], with units in meters and radians.

    • addVisionMeasurement

      public void addVisionMeasurement(org.wpilib.math.geometry.Pose2d robotPose, double timestamp)
      Add a vision measurement to the SwerveDrivePoseEstimator and update the gyro reading with the given timestamp of the vision measurement.
      Parameters:
      robotPose - Robot Pose2d as measured by vision.
      timestamp - Timestamp the measurement was taken as time since startup, should be taken from Timer.getTimestamp() or similar sources.

    • updateTelemetry

      public void updateTelemetry()
      Update the telemetry and SwerveDrivePoseEstimator of the drive.

    • simIterate

      public void simIterate()
      Simulate the drive, updating the gyroscope based off of module states.

    • getRobotRelativeSpeed

      public org.wpilib.math.kinematics.ChassisVelocities getRobotRelativeSpeed()
      Get the robot relative speed of the drive.
      Returns:
      Robot relative speed of the drive.

    • getFieldRelativeSpeed

      public org.wpilib.math.kinematics.ChassisVelocities getFieldRelativeSpeed()
      Get the field relative speed of the drive.
      Returns:
      Field relative speed of the drive.

    • getModulePositions

      public org.wpilib.math.kinematics.SwerveModulePosition[] getModulePositions()
      Get the SwerveModulePosition of the modules.
      Returns:
      SwerveModulePosition of the modules.

    • getModuleStates

      public org.wpilib.math.kinematics.SwerveModuleVelocity[] getModuleStates()
      Get the SwerveModuleVelocity of the modules.
      Returns:
      SwerveModuleVelocity of the modules.

    • getConfig

      public SwerveDriveConfig<?> getConfig()
      Get the SwerveDriveConfig of the drive.
      Returns:
      SwerveDriveConfig of the drive.

    • getField2d

      public org.wpilib.smartdashboard.Field2d getField2d()
      Get the Field2d used to display the robot's pose, so callers (e.g. vision subsystems) can publish additional FieldObject2ds onto the same field widget instead of creating their own.
      Returns:
      Field2d of the drive.

    • getDesiredChassisSpeeds

      public org.wpilib.math.kinematics.ChassisVelocities getDesiredChassisSpeeds()
      Get the last-commanded desired robot-relative ChassisVelocities of the drive. This is the value cached by setRobotRelativeChassisSpeeds(ChassisVelocities) (which setFieldRelativeChassisSpeeds(ChassisVelocities) and SwerveDrive.drive(java.util.function.Supplier) funnel through) and published on every updateTelemetry() call.
      Returns:
      Robot-relative ChassisVelocities last commanded to the drive. Defaults to a zeroed ChassisVelocities if the drive has never been commanded.
      Implementation Note:
      It is a setpoint, not a measurement of actual robot motion. Use getRobotRelativeSpeed() or getFieldRelativeSpeed() instead if you need the drive's actual measured speed.

    • getDesiredModuleStates

      public org.wpilib.math.kinematics.SwerveModuleVelocity[] getDesiredModuleStates()
      Get the last-commanded desired SwerveModuleVelocitys of the drive.
      Returns:
      SwerveModuleVelocitys last commanded to the drive. Defaults to a zeroed SwerveModuleVelocitys if the drive has never been commanded.

    • getModule

      public Optional<SwerveModule> getModule(String moduleName)
      Get a module by its name.
      Parameters:
      moduleName - Name of the module.
      Returns:
      SwerveModule with the given name if it exists.

    • getModules

      public SwerveModule[] getModules()
      Get all SwerveModules for this SwerveDrive.
      Returns:
      Array of SwerveModules.