Class SwerveInputStream

java.lang.Object
yams.commands2.swerve.SwerveInputStream
All Implemented Interfaces:
Supplier<org.wpilib.math.kinematics.ChassisVelocities>
public class SwerveInputStream extends Object implements Supplier<org.wpilib.math.kinematics.ChassisVelocities>
Helper class to easily transform controller inputs into workable chassis speeds. Inspired by SciBorgs FRC 1155.

Joystick-to-SwerveDrive adapter

SwerveInputStream acts as a bridge between raw controller axis values (in the range [-1, 1]) and the ChassisVelocities that SwerveDrive expects. It handles deadbands, axis scaling, non-linear (cubed) response curves, field-relative / alliance-relative flipping, and multiple drive modes (angular-velocity, heading-snap, translation-only, aim-at-target). Because it implements Supplier<ChassisVelocities> it can be passed directly to a command that calls swerveDrive.drive(inputStream.get()).

Typical usage with an NiDsXboxController

NiDsXboxController driver = new NiDsXboxController(0);

// Angular-velocity stream: left stick translates, right stick X rotates
SwerveInputStream angularVelocityStream =
    SwerveInputStream.of(swerveDrive,
                         () -> -driver.getLeftY(),   // forward/back
                         () -> -driver.getLeftX())   // strafe
                     .withControllerRotationAxis(driver::getRightX)
                     .withDeadband(0.05)
                     .withScaleTranslation(0.8)
                     .withScaleRotation(0.6)
                     .withAllianceRelativeControl()  // auto-flip for Red alliance
                     .withCubeTranslationControllerAxis(); // non-linear response

// Heading-snap variant: right stick X/Y picks a desired heading angle
SwerveInputStream headingStream = angularVelocityStream.clone()
    .withControllerHeadingAxis(driver::getRightX, driver::getRightY)
    .withHeadingControl(() -> driver.getRightStickButton());

// In your periodic or a command, call .get() to obtain field relative ChassisVelocities:
swerveDrive.setFieldRelativeChassisSpeeds(angularVelocityStream.get());

  • Constructor Details

  • Method Details

    • of

      public static SwerveInputStream of(SwerveDrive drive, DoubleSupplier x, DoubleSupplier y)
      Create a SwerveInputStream with only translation axes. Add rotation with withControllerRotationAxis(DoubleSupplier) or withControllerHeadingAxis(DoubleSupplier, DoubleSupplier).
      Parameters:
      drive - SwerveDrive the stream drives.
      x - Field relative X (forward) translation axis, in [-1, 1].
      y - Field relative Y (left) translation axis, in [-1, 1].
      Returns:
      A new SwerveInputStream.

    • of

      public static SwerveInputStream of(SwerveDrive drive, DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot)
      Create a SwerveInputStream that rotates the robot with an angular velocity axis.
      Parameters:
      drive - SwerveDrive the stream drives.
      x - Field relative X (forward) translation axis, in [-1, 1].
      y - Field relative Y (left) translation axis, in [-1, 1].
      rot - Angular velocity axis, counter-clockwise positive, in [-1, 1].
      Returns:
      A new SwerveInputStream.

    • clone

      public SwerveInputStream clone()
      Overrides:
      clone in class Object

    • getCurrentModeName

      public String getCurrentModeName()
      Get the name of the drive mode used by the last get() call.
      Returns:
      Mode name, e.g. ANGULAR_VELOCITY, HEADING, AIM or TRANSLATION_ONLY.

    • getAxisDeadband

      public double getAxisDeadband()
      Get the controller axis deadband.
      Returns:
      Axis deadband, 0 when none is applied.

    • setAxisDeadband

      public void setAxisDeadband(double deadband)
      Set the controller axis deadband.
      Parameters:
      deadband - Deadband applied to every axis, 0 to disable.

    • getTranslationAxisScale

      public double getTranslationAxisScale()
      Get the translation axis scalar.
      Returns:
      Translation axis scalar, 1 when unscaled.

    • setTranslationAxisScale

      public void setTranslationAxisScale(double scale)
      Set the translation axis scalar.
      Parameters:
      scale - Scalar in (0, 1] applied to the translation axes, 0 to disable.

    • getOmegaAxisScale

      public double getOmegaAxisScale()
      Get the angular velocity axis scalar.
      Returns:
      Angular velocity axis scalar, 1 when unscaled.

    • setOmegaAxisScale

      public void setOmegaAxisScale(double scale)
      Set the angular velocity axis scalar.
      Parameters:
      scale - Scalar in (0, 1] applied to the angular velocity axis, 0 to disable.

    • getMaximumChassisLinearVelocity

      public org.wpilib.units.measure.LinearVelocity getMaximumChassisLinearVelocity()
      Get the chassis linear velocity a full translation axis maps to.
      Returns:
      Maximum chassis linear velocity.

    • setMaximumChassisLinearVelocity

      public void setMaximumChassisLinearVelocity(org.wpilib.units.measure.LinearVelocity velocity)
      Set the chassis linear velocity a full translation axis maps to.
      Parameters:
      velocity - Maximum chassis linear velocity.

    • getMaximumChassisAngularVelocity

      public org.wpilib.units.measure.AngularVelocity getMaximumChassisAngularVelocity()
      Get the chassis angular velocity a full angular velocity axis maps to.
      Returns:
      Maximum chassis angular velocity.

    • setMaximumChassisAngularVelocity

      public void setMaximumChassisAngularVelocity(org.wpilib.units.measure.AngularVelocity velocity)
      Set the chassis angular velocity a full angular velocity axis maps to.
      Parameters:
      velocity - Maximum chassis angular velocity.

    • isTranslationCubeEnabled

      public boolean isTranslationCubeEnabled()
      Check if the translation magnitude is cubed, for finer control near the center of the stick.
      Returns:
      True if translation cubing is enabled.

    • setTranslationCubeEnabled

      public void setTranslationCubeEnabled(boolean enabled)
      Enable or disable cubing the translation magnitude.
      Parameters:
      enabled - True to cube the translation magnitude.

    • isOmegaCubeEnabled

      public boolean isOmegaCubeEnabled()
      Check if the angular velocity axis is cubed, for finer control near the center of the stick.
      Returns:
      True if angular velocity cubing is enabled.

    • setOmegaCubeEnabled

      public void setOmegaCubeEnabled(boolean enabled)
      Enable or disable cubing the angular velocity axis.
      Parameters:
      enabled - True to cube the angular velocity axis.

    • isAllianceRelativeEnabled

      public boolean isAllianceRelativeEnabled()
      Check if translation is flipped 180 degrees while on the red alliance.
      Returns:
      True if alliance relative control is enabled.

    • setAllianceRelativeEnabled

      public void setAllianceRelativeEnabled(boolean enabled)
      Enable or disable flipping translation 180 degrees while on the red alliance.
      Parameters:
      enabled - True to enable alliance relative control.

    • isRobotRelativeEnabled

      public boolean isRobotRelativeEnabled()
      Check if the translation axes are treated as robot relative.
      Returns:
      True if robot relative control is enabled.

    • setRobotRelativeEnabled

      public void setRobotRelativeEnabled(boolean enabled)
      Enable or disable treating the translation axes as robot relative.
      Parameters:
      enabled - True to enable robot relative control.

    • withTelemetry

      public SwerveInputStream withTelemetry(String name, TelemetryVerbosity verbosity)
      Publish this stream's telemetry under SwerveInputStream/<name>, updated every time the stream is read. At TelemetryVerbosity.HIGH the stream is also live tunable: see SwerveInputStreamTelemetry. Replaces any telemetry this stream already had. A clone() does not copy the telemetry.
      Parameters:
      name - Name of the stream in NetworkTables (e.g., "drive").
      verbosity - TelemetryVerbosity to publish at.
      Returns:
      this, for chaining.

    • getTelemetry

      public Optional<SwerveInputStreamTelemetry> getTelemetry()
      Get this stream's telemetry, created by withTelemetry(String, TelemetryVerbosity).
      Returns:
      The SwerveInputStreamTelemetry, or empty if telemetry is not enabled.

    • withMaximumLinearVelocity

      public SwerveInputStream withMaximumLinearVelocity(org.wpilib.units.measure.LinearVelocity velocity)
      Set the chassis linear velocity a full translation axis maps to. Defaults to the drive's maximum chassis linear velocity.
      Parameters:
      velocity - Maximum chassis linear velocity.
      Returns:
      this, for chaining.

    • withMaximumAngularVelocity

      public SwerveInputStream withMaximumAngularVelocity(org.wpilib.units.measure.AngularVelocity velocity)
      Set the chassis angular velocity a full angular velocity axis maps to. Defaults to the drive's maximum chassis angular velocity.
      Parameters:
      velocity - Maximum chassis angular velocity.
      Returns:
      this, for chaining.

    • withRobotRelative

      public SwerveInputStream withRobotRelative(BooleanSupplier enabled)
      Treat the translation axes as robot relative while enabled. The output is still field relative, and alliance relative flipping is skipped.
      Parameters:
      enabled - Robot relative control is used while this is true.
      Returns:
      this, for chaining.

    • withRobotRelative

      public SwerveInputStream withRobotRelative()
      Always treat the translation axes as robot relative.
      Returns:
      this, for chaining.

    • withTranslationHeadingOffset

      public SwerveInputStream withTranslationHeadingOffset(org.wpilib.math.geometry.Rotation2d angle, BooleanSupplier enabled)
      Rotate the output translation by an offset while enabled.
      Parameters:
      angle - Offset to rotate the translation by.
      enabled - The offset is applied while this is true.
      Returns:
      this, for chaining.

    • withTranslationHeadingOffset

      public SwerveInputStream withTranslationHeadingOffset(org.wpilib.math.geometry.Rotation2d angle)
      Always rotate the output translation by an offset.
      Parameters:
      angle - Offset to rotate the translation by.
      Returns:
      this, for chaining.

    • withAllianceRelativeControl

      public SwerveInputStream withAllianceRelativeControl()
      Always flip translation 180 degrees while on the red alliance, so forward is away from the driver station.
      Returns:
      this, for chaining.

    • withAllianceRelativeControl

      public SwerveInputStream withAllianceRelativeControl(BooleanSupplier enabled)
      Flip translation 180 degrees while on the red alliance, so forward is away from the driver station.
      Parameters:
      enabled - Alliance relative control is used while this is true.
      Returns:
      this, for chaining.

    • withCubeRotationControllerAxis

      public SwerveInputStream withCubeRotationControllerAxis(BooleanSupplier enabled)
      Cube the angular velocity axis while enabled, for finer control near the center of the stick.
      Parameters:
      enabled - The axis is cubed while this is true.
      Returns:
      this, for chaining.

    • withCubeRotationControllerAxis

      public SwerveInputStream withCubeRotationControllerAxis()
      Always cube the angular velocity axis, for finer control near the center of the stick.
      Returns:
      this, for chaining.

    • withCubeTranslationControllerAxis

      public SwerveInputStream withCubeTranslationControllerAxis(BooleanSupplier enabled)
      Cube the translation magnitude while enabled, for finer control near the center of the stick.
      Parameters:
      enabled - The magnitude is cubed while this is true.
      Returns:
      this, for chaining.

    • withCubeTranslationControllerAxis

      public SwerveInputStream withCubeTranslationControllerAxis()
      Always cube the translation magnitude, for finer control near the center of the stick.
      Returns:
      this, for chaining.

    • withControllerRotationAxis

      public SwerveInputStream withControllerRotationAxis(DoubleSupplier rot)
      Set the axis used for angular velocity control.
      Parameters:
      rot - Angular velocity axis, counter-clockwise positive, in [-1, 1].
      Returns:
      this, for chaining.

    • withControllerHeadingAxis

      public SwerveInputStream withControllerHeadingAxis(DoubleSupplier headingX, DoubleSupplier headingY)
      Face the direction a controller stick points while heading control is enabled. The robot stops turning while the stick is inside the deadband. Replaces any heading from withHeading(Supplier).
      Parameters:
      headingX - Heading stick X axis, in [-1, 1].
      headingY - Heading stick Y axis, in [-1, 1].
      Returns:
      this, for chaining.

    • deadband

      public SwerveInputStream deadband(double deadband)
      Set the deadband applied to every controller axis.
      Parameters:
      deadband - Axis deadband, 0 to disable.
      Returns:
      this, for chaining.

    • withDeadband

      public SwerveInputStream withDeadband(double deadband)
      Set the deadband applied to every controller axis.
      Parameters:
      deadband - Axis deadband, 0 to disable.
      Returns:
      this, for chaining.

    • withScaleTranslation

      public SwerveInputStream withScaleTranslation(double scaleTranslation)
      Scale the translation axes.
      Parameters:
      scaleTranslation - Scalar in (0, 1], 0 to disable.
      Returns:
      this, for chaining.

    • withScaleRotation

      public SwerveInputStream withScaleRotation(double scaleRotation)
      Scale the angular velocity axis.
      Parameters:
      scaleRotation - Scalar in (0, 1], 0 to disable.
      Returns:
      this, for chaining.

    • withHeadingControl

      public SwerveInputStream withHeadingControl(BooleanSupplier trigger)
      Face the heading from withHeading(Supplier) or withControllerHeadingAxis(DoubleSupplier, DoubleSupplier) while enabled. Requires a rotation controller on the SwerveDrive.
      Parameters:
      trigger - Heading control is used while this is true.
      Returns:
      this, for chaining.

    • withHeading

      public SwerveInputStream withHeading(Supplier<org.wpilib.units.measure.Angle> heading)
      Set the field relative heading to face while heading control is enabled. Replaces any controller heading axis.
      Parameters:
      heading - Field relative heading supplier.
      Returns:
      this, for chaining.

    • withAim

      public SwerveInputStream withAim(Supplier<org.wpilib.math.geometry.Pose2d> aimTarget, BooleanSupplier trigger)
      Face a target pose while enabled. Requires a rotation controller on the SwerveDrive.
      Parameters:
      aimTarget - Field relative pose to aim at, null for no target.
      trigger - Aiming is used while this is true.
      Returns:
      this, for chaining.

    • withAimLookahead

      public SwerveInputStream withAimLookahead(org.wpilib.units.measure.Time lookahead)
      Aim from where the robot will be after a delay, instead of where it is now. The pose is moved along the robot's field relative velocity by this time before the direction to the target is worked out.

      A game piece leaves the robot a moment after the aim was calculated: the camera, the loop, the motors and the feed all take time, and the robot keeps moving through all of it. Add those delays together for a starting value, then tune it. It is often 100 to 200 milliseconds.

      This assumes the robot keeps its current velocity, so it works best while driving in a straight line.

      Parameters:
      lookahead - How far ahead in time to estimate the robot's position, null for none.
      Returns:
      this, for chaining.

    • withAimFeedforward

      public SwerveInputStream withAimFeedforward(org.wpilib.math.controller.SimpleMotorFeedforward feedforward)
      Add a feedforward to the aim rotation, so the robot keeps up with a target whose direction is changing. The feedforward is calculated from the robot's current angular velocity to the angular velocity the rotation controller asks for, and its result is added to that angular velocity.
      Parameters:
      feedforward - Feedforward in radians, null for none.
      Returns:
      this, for chaining.

    • withAimHeadingOffset

      public SwerveInputStream withAimHeadingOffset(org.wpilib.math.geometry.Rotation2d offset, BooleanSupplier enabled)
      Rotate the aim heading by an offset while enabled, for a shooter that does not point out of the front of the robot.
      Parameters:
      offset - Offset added to the heading that faces the target, counter-clockwise positive.
      enabled - The offset is applied while this is true.
      Returns:
      this, for chaining.

    • withAimHeadingOffset

      public SwerveInputStream withAimHeadingOffset(org.wpilib.math.geometry.Rotation2d offset)
      Always rotate the aim heading by an offset, for a shooter that does not point out of the front of the robot.
      Parameters:
      offset - Offset added to the heading that faces the target, counter-clockwise positive.
      Returns:
      this, for chaining.

    • isAimLocked

      public boolean isAimLocked(org.wpilib.units.measure.Angle tolerance)
      Check if the robot is aiming and facing its aim heading. Only updates while the stream is being read.
      Parameters:
      tolerance - How far the robot's heading may be from the aim heading.
      Returns:
      True while aiming is enabled and the heading is within the tolerance.

    • aimLock

      public org.wpilib.command2.button.Trigger aimLock(org.wpilib.units.measure.Angle tolerance)
      Trigger that is true while the robot is aiming and facing its aim heading.
      Parameters:
      tolerance - How far the robot's heading may be from the aim heading.
      Returns:
      Trigger for the aim lock.

    • getTargetVector

      public org.wpilib.math.geometry.Translation2d getTargetVector(org.wpilib.math.geometry.Pose2d target)
      Get the field relative vector from the robot to a target, from where the robot will be after the withAimLookahead(Time) time if there is one. Useful for shooting on the move.
      Parameters:
      target - Field relative pose of the target.
      Returns:
      Field relative Translation2d from the robot to the target.

    • calculateAngularVelocity

      public org.wpilib.units.measure.AngularVelocity calculateAngularVelocity(org.wpilib.units.measure.Angle target)
      Calculate the angular velocity that turns the robot to a heading, with the SwerveDrive's rotation controller and the withAimFeedforward(SimpleMotorFeedforward) feedforward if there is one.
      Parameters:
      target - Heading to face, in the same frame as the gyro.
      Returns:
      AngularVelocity that turns the robot toward the heading.

    • withTranslationOnly

      public SwerveInputStream withTranslationOnly(BooleanSupplier trigger)
      Hold the heading the robot had when enabled and only translate. Requires a rotation controller on the SwerveDrive.
      Parameters:
      trigger - Translation only control is used while this is true.
      Returns:
      this, for chaining.

    • get

      public org.wpilib.math.kinematics.ChassisVelocities get()
      Specified by:
      get in interface Supplier<org.wpilib.math.kinematics.ChassisVelocities>