Class SwerveDriveConfig<T extends SwerveDriveConfig<T>>

java.lang.Object
yams.core.mechanisms.config.SwerveDriveConfig<T>
Type Parameters:
T - Concrete config type, so chained with calls keep the subclass type.
Direct Known Subclasses:
SwerveDriveConfig, SwerveDriveConfig
public abstract class SwerveDriveConfig<T extends SwerveDriveConfig<T>> extends Object
Swerve Drive Configuration.

Holds modules, gyro supplier, and drive controllers. Modules are ordered clockwise from front-left: FL, FR, BL, BR. See SwerveDriveConfig for the Subsystem-bound subclass and a full construction example.

  • Constructor Details

  • Method Details

    • self

      protected abstract T self()
      Get this config as its concrete type, so chained with calls keep the subclass type.
      Returns:
      this SwerveDriveConfig.

    • clone

      public abstract T clone()
      Clone the SwerveDriveConfig without modules, subsystem, telemetry name, gyro supplier, gyro angular velocity supplier, gyro offset, and gyro inversion.
      Overrides:
      clone in class Object
      Returns:
      New SwerveDriveConfig

    • withModules

      public T withModules(SwerveModule... modules)
      Set the SwerveModules for the SwerveDrive.
      Parameters:
      modules - SwerveModules for the SwerveDrive.
      Returns:
      SwerveDriveConfig for chaining.

    • withSimTranslationController

      public T withSimTranslationController(org.wpilib.math.controller.PIDController controller)
      Set the translation PID controller.
      Parameters:
      controller - PIDController for the translation, input units are meters.
      Returns:
      SwerveDriveConfig for chaining.

    • withSimRotationController

      public T withSimRotationController(org.wpilib.math.controller.PIDController controller)
      Set the rotation PID controller.
      Parameters:
      controller - PIDController for the rotation, input units are radians.
      Returns:
      SwerveDriveConfig for chaining.

    • withTranslationController

      public T withTranslationController(org.wpilib.math.controller.PIDController controller)
      Set the translation PID controller.
      Parameters:
      controller - PIDController for the translation, input units are meters.
      Returns:
      SwerveDriveConfig for chaining.

    • withRotationController

      public T withRotationController(org.wpilib.math.controller.PIDController controller)
      Set the rotation PID controller.
      Parameters:
      controller - PIDController for the rotation, input units are radians.
      Returns:
      SwerveDriveConfig for chaining.

    • withCenterOfRotation

      public T withCenterOfRotation(org.wpilib.math.geometry.Translation2d centerOfRotation)
      Set the center of rotation; 0,0 is the center of the robot.
      Parameters:
      centerOfRotation - Translation2d of the center of rotation in Meters, X is forward, Y is left.
      Returns:
      SwerveDriveConfig for chaining.

    • withCenterOfRotation

      public T withCenterOfRotation(org.wpilib.units.measure.Distance forward, org.wpilib.units.measure.Distance left)
      Set the center of rotation; 0,0 is the center of the robot.
      Parameters:
      forward - Forward distance from the center of robot.
      left - Left distance from the center of robot.
      Returns:
      SwerveDriveConfig for chaining.

    • withSimDiscretizationTime

      public T withSimDiscretizationTime(org.wpilib.units.measure.Time dt)
      Set the discretization time for the pose estimation.
      Parameters:
      dt - Discretization time for the pose estimation.
      Returns:
      SwerveDriveConfig for chaining.

    • withSimGyroAngularVelocityScaleFactor

      public T withSimGyroAngularVelocityScaleFactor(double scaleFactor)
      Set the angular velocity scale factor to improve the accuracy of the pose estimation.
      Parameters:
      scaleFactor - Scale factor to apply to the gyro angular velocity, [0, 1].
      Returns:
      SwerveDriveConfig for chaining.

    • withDiscretizationTime

      public T withDiscretizationTime(org.wpilib.units.measure.Time dt)
      Set the discretization time for the pose estimation.
      Parameters:
      dt - Discretization time for the pose estimation.
      Returns:
      SwerveDriveConfig for chaining.

    • withGyroAngularVelocityScaleFactor

      public T withGyroAngularVelocityScaleFactor(double scaleFactor)
      Set the angular velocity scale factor to improve the accuracy of the pose estimation.
      Parameters:
      scaleFactor - Scale factor to apply to the gyro angular velocity, [0, 1].
      Returns:
      SwerveDriveConfig for chaining.

    • withGyroVelocity

      public T withGyroVelocity(Supplier<org.wpilib.units.measure.AngularVelocity> angularVelocitySupplier)
      Angular velocity of the gyro.
      Parameters:
      angularVelocitySupplier - Supplier<org.wpilib.units.measure.AngularVelocity> for the gyro angular velocity.
      Returns:
      SwerveDriveConfig for chaining.

    • withGyro

      public T withGyro(Supplier<org.wpilib.math.geometry.Rotation3d> gyro)
      Set the gyro, as the robot's attitude: its yaw is the robot's heading (or the axis set with withGyroHeadingAxis(GyroAxis)), and its roll and pitch are what anti-tipping corrects with (see withAntiTipping(AntiTipping)).
      Parameters:
      gyro - Supplier for the robot's attitude, such as pigeon::getRotation3d or imu::getRotation3d.
      Returns:
      SwerveDriveConfig for chaining.

    • withGyroHeadingAxis

      public T withGyroHeadingAxis(GyroAxis axis)
      Set which axis of the gyro points up through the robot, for a gyro mounted so the robot turning shows up as its roll or pitch instead of its yaw. Defaults to GyroAxis.YAW. The gyro's attitude is rotated into the robot's frame with GyroAxis.toRobotAttitude(Rotation3d) before the heading and anti-tipping (see withAntiTipping(AntiTipping)) read it; withGyroInverted(boolean) and withGyroOffset(Angle) apply to the resulting heading.
      Parameters:
      axis - Axis of the gyro that points up through the robot.
      Returns:
      SwerveDriveConfig for chaining.

    • withAntiTipping

      public T withAntiTipping(org.wpilib.units.measure.LinearVelocity kP, org.wpilib.units.measure.Angle tippingThreshold, org.wpilib.units.measure.LinearVelocity maxCorrectionSpeed)
      Drive the robot back under itself when it starts to tip, with WPILib's AntiTipping: once the robot's pitch or roll, from the gyro set with withGyro(Supplier), is past tippingThreshold, a correction toward the side it is tipping to, kP times the sine of the tilt and at most maxCorrectionSpeed, is added to every robot relative chassis speed. See AntiTipping for tuning.
      Parameters:
      kP - Correction speed per sine of the tilt.
      tippingThreshold - Tilt past which to correct: a few degrees above the most the robot tilts in normal driving, well below where it tips.
      maxCorrectionSpeed - Fastest the correction may drive.
      Returns:
      SwerveDriveConfig for chaining.
      Throws:
      IllegalArgumentException - if kP or maxCorrectionSpeed is not positive, or tippingThreshold is not between 0 and 90 degrees.

    • withAntiTipping

      public T withAntiTipping(org.wpilib.math.controller.AntiTipping antiTipping)
      Drive the robot back under itself when it starts to tip, with an AntiTipping reading the robot's attitude from the gyro set with withGyro(Supplier): its correction is added to every robot relative chassis speed.
      Parameters:
      antiTipping - AntiTipping to correct with.
      Returns:
      SwerveDriveConfig for chaining.

    • getAntiTippingCorrection

      public org.wpilib.math.kinematics.ChassisVelocities getAntiTippingCorrection()
      Get the anti-tipping correction for the robot's attitude now.
      Returns:
      Robot relative correction, or zero when the robot is not tipping, or anti-tipping or the gyro is not configured.

    • withGyroOffset

      public T withGyroOffset(org.wpilib.units.measure.Angle offset)
      Set the gyro offset.
      Parameters:
      offset - Offset to apply to the gyro.
      Returns:
      SwerveDriveConfig for chaining.

    • withGyroInverted

      public T withGyroInverted(boolean inverted)
      Set the gyro inverted.
      Parameters:
      inverted - Inverted state of the gyro.
      Returns:
      SwerveDriveConfig for chaining.

    • withMaximumChassisSpeed

      public T withMaximumChassisSpeed(org.wpilib.units.measure.LinearVelocity speed, org.wpilib.units.measure.AngularVelocity angularVelocity)
      Maximum speed of the chassis to desaturate towards.
      Parameters:
      speed - Linear velocity of the Chassis.
      angularVelocity - Angular velocity of the Chassis.
      Returns:
      SwerveDriveConfig for chaining.

    • withMaximumModuleSpeed

      public T withMaximumModuleSpeed(org.wpilib.units.measure.LinearVelocity speed)
      Set the maximum speed of the modules to desaturate towards.
      Parameters:
      speed - Linear velocity of the modules.
      Returns:
      SwerveDriveConfig for chaining.

    • withModuleStateOptimization

      public T withModuleStateOptimization(boolean enabled)
      Enable or disable state optimization on every SwerveModule of the drive, overriding each module's SwerveModuleConfig.withOptimization(boolean). Without this override, each module keeps its own setting.

      Modules already in this config are updated immediately, so calling this on a running drive's config (e.g. drive.getConfig().withModuleStateOptimization(false)) toggles optimization during operation. Modules added later with withModules(SwerveModule...) get it when the SwerveDrive is created.

      Parameters:
      enabled - True to let modules turn around and reverse their drive when that is the shorter way to the requested angle, false to always steer to the requested angle.
      Returns:
      SwerveDriveConfig for chaining.

    • withStartingPose

      public T withStartingPose(org.wpilib.math.geometry.Pose2d pose)
      Set the starting pose of the robot.
      Parameters:
      pose - Pose2d to set the robot to. new Pose2d()
      Returns:
      SwerveDriveConfig for chaining.

    • withTelemetry

      public T withTelemetry(String name, TelemetryVerbosity telemetryVerbosity)
      Configure telemetry for the SwerveModule mechanism.
      Parameters:
      name - Telemetry Name
      telemetryVerbosity - Telemetry verbosity to apply.
      Returns:
      SwerveDriveConfig for chaining.

    • withTelemetry

      public T withTelemetry(String name, SwerveDriveTelemetryConfig telemetryConfig)
      Configure telemetry for the SwerveDrive with a SwerveDriveTelemetryConfig.
      Parameters:
      name - Telemetry Name
      telemetryConfig - Config that specifies what to log.
      Returns:
      SwerveDriveConfig for chaining.

    • getSwerveDriveTelemetryConfig

      public Optional<SwerveDriveTelemetryConfig> getSwerveDriveTelemetryConfig()
      Get the user specified SwerveDriveTelemetryConfig, if configured via withTelemetry(String,SwerveDriveTelemetryConfig).
      Returns:
      SwerveDriveTelemetryConfig if configured.

    • getTelemetryName

      public String getTelemetryName()
      Get the telemetry name for the SwerveDrive.
      Returns:
      Telemetry name for the SwerveDrive.

    • getCenterOfRotation

      public Optional<org.wpilib.math.geometry.Translation2d> getCenterOfRotation()
      Get the center of rotation.
      Returns:
      Translation2d of the center of rotation in Meters, X is forward, Y is left.

    • getTelemetryVerbosity

      public Optional<TelemetryVerbosity> getTelemetryVerbosity()
      Get the telemetry verbosity for the SwerveModule.
      Returns:
      TelemetryVerbosity for the SwerveModule.

    • getModules

      public SwerveModule[] getModules()
      Get the SwerveModules for the SwerveDrive.
      Returns:
      SwerveModules for the SwerveDrive.

    • getGyroRotation3d

      public org.wpilib.math.geometry.Rotation3d getGyroRotation3d()
      Get the robot's attitude from the gyro: the gyro's attitude rotated into the robot's frame for the axis set with withGyroHeadingAxis(GyroAxis), with the inversion (withGyroInverted(boolean)) and offset (withGyroOffset(Angle)) applied to its yaw. Its yaw is the robot's heading, wrapped to half a rotation each way, and its roll and pitch are the robot's tilt.
      Returns:
      The robot's attitude.
      Throws:
      IllegalStateException - if no gyro supplier was set with withGyro(Supplier).

    • getInitialPose

      public org.wpilib.math.geometry.Pose2d getInitialPose()
      Get the starting pose of the robot.
      Returns:
      Pose2d of the robot.

    • getMaximumChassisLinearVelocity

      public Optional<org.wpilib.units.measure.LinearVelocity> getMaximumChassisLinearVelocity()
      Get the maximum speed of the chassis.
      Returns:
      Maximum speed of the chassis.

    • getMaximumChassisAngularVelocity

      public Optional<org.wpilib.units.measure.AngularVelocity> getMaximumChassisAngularVelocity()
      Get the maximum angular speed of the chassis.
      Returns:
      Maximum angular speed of the chassis.

    • getMaximumModuleLinearVelocity

      public Optional<org.wpilib.units.measure.LinearVelocity> getMaximumModuleLinearVelocity()
      Get the maximum speed of the modules.
      Returns:
      Maximum speed of the modules.

    • getModuleStateOptimization

      public Optional<Boolean> getModuleStateOptimization()
      Get the module state optimization override.
      Returns:
      True or false to enable or disable optimization on every module, or empty to leave each module's own setting.

    • optimizeRobotRelativeChassisSpeeds

      public org.wpilib.math.kinematics.ChassisVelocities optimizeRobotRelativeChassisSpeeds(org.wpilib.math.kinematics.ChassisVelocities speeds)
      Optimize the given chassis speeds, first adding the anti-tipping correction when it is configured with withAntiTipping(AntiTipping).
      Parameters:
      speeds - ChassisVelocities to optimize.
      Returns:
      Optimized ChassisVelocities.
      Throws:
      IllegalStateException - if a gyro angular velocity scale factor is configured with withGyroAngularVelocityScaleFactor(double) but no gyro supplier was set with withGyro(Supplier).

    • getGyroOffset

      public org.wpilib.units.measure.Angle getGyroOffset()
      Get the gyro offset.
      Returns:
      Gyro offset.

    • getTranslationPID

      public Optional<org.wpilib.math.controller.PIDController> getTranslationPID()
      Get the translation PID controller used for drive to pose. In simulation the simulation controller is preferred over the real one.
      Returns:
      Translation PID controller, or empty when none is configured.

    • getRotationPID

      public Optional<org.wpilib.math.controller.PIDController> getRotationPID()
      Get the rotation PID controller used for drive to pose and heading control. In simulation the simulation controller is preferred over the real one.
      Returns:
      Rotation PID controller, or empty when none is configured.

    • cubeTranslation

      public static org.wpilib.math.geometry.Translation2d cubeTranslation(org.wpilib.math.geometry.Translation2d translation)
      Cube the Translation2d magnitude given in Polar coordinates.
      Parameters:
      translation - Translation2d to manipulate.
      Returns:
      Cubed magnitude from Translation2d.

    • scaleTranslation

      public static org.wpilib.math.geometry.Translation2d scaleTranslation(org.wpilib.math.geometry.Translation2d translation, double scalar)
      Scale the Translation2d Polar coordinate magnitude.
      Parameters:
      translation - Translation2d to use.
      scalar - Multiplier for the Polar coordinate magnitude to use.
      Returns:
      Translation2d scaled by given magnitude scalar.

    • getDiscretization

      public Optional<org.wpilib.units.measure.Time> getDiscretization()
      Get the discretization time for the pose estimation used by optimizeRobotRelativeChassisSpeeds(ChassisVelocities)
      Returns:
      Discretization time for the pose estimation.

    • useExternalFeedbackSensor

      public boolean useExternalFeedbackSensor()
      Use an external feedback sensor for the SwerveModules.
      Returns:
      External feedback sensor for the SwerveModules.