Package swervelib

Class SwerveInputStream

java.lang.Object
swervelib.SwerveInputStream
All Implemented Interfaces:
Supplier<ChassisSpeeds>
public class SwerveInputStream extends Object implements Supplier<ChassisSpeeds>
Helper class to easily transform Controller inputs into workable Chassis speeds. Intended to easily create an interface that generates ChassisSpeeds from XboxController


Inspired by SciBorgs FRC 1155.
Example: 

 
   XboxController driverXbox = new XboxController(0);

   SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
                                                                 () -> driverXbox.getLeftY() * -1,
                                                                 () -> driverXbox.getLeftX() * -1) // Axis which give the desired translational angle and speed.
                                                             .withControllerRotationAxis(driverXbox::getRightX) // Axis which give the desired angular velocity.
                                                             .deadband(0.01)                  // Controller deadband
                                                             .scaleTranslation(0.8)           // Scaled controller translation axis
                                                             .allianceRelativeControl(true);  // Alliance relative controls.

   SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()  // Copy the stream so further changes do not affect driveAngularVelocity
                                                            .withControllerHeadingAxis(driverXbox::getRightX,
                                                                                       driverXbox::getRightY) // Axis which give the desired heading angle using trigonometry.
                                                            .headingWhile(true); // Enable heading based control.

  • Constructor Details

  • Method Details

    • of

      public static SwerveInputStream of(SwerveDrive drive, DoubleSupplier x, DoubleSupplier y)
      Create basic SwerveInputStream without any rotation components.
      Parameters:
      drive - SwerveDrive object for transformation.
      x - DoubleSupplier of the translation X axis of the controller joystick to use.
      y - DoubleSupplier of the translation X axis of the controller joystick to use.
      Returns:
      SwerveInputStream to use as you see fit.

    • copy

      public SwerveInputStream copy()
      Copy the SwerveInputStream object.
      Returns:
      Clone of current SwerveInputStream

    • robotRelative

      public SwerveInputStream robotRelative(BooleanSupplier enabled)
      Set the stream to output robot relative ChassisSpeeds
      Parameters:
      enabled - Robot-Relative ChassisSpeeds output.
      Returns:
      self

    • robotRelative

      public SwerveInputStream robotRelative(boolean enabled)
      Set the stream to output robot relative ChassisSpeeds
      Parameters:
      enabled - Robot-Relative ChassisSpeeds output.
      Returns:
      self

    • driveToPose

      public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController xPIDController, ProfiledPIDController omegaPIDController)
      Drive to a given pose with the provided ProfiledPIDControllers
      Parameters:
      pose - Supplier<Pose2d> for ease of use.
      xPIDController - PID controller for the translational axis, units are m/s.
      omegaPIDController - PID Controller for rotational axis, units are rad/s.
      Returns:
      self

    • driveToPoseEnabled

      public SwerveInputStream driveToPoseEnabled(BooleanSupplier enabled)
      Enable driving to the target pose.
      Parameters:
      enabled - Enable state of drive to pose.
      Returns:
      self.

    • driveToPoseEnabled

      public SwerveInputStream driveToPoseEnabled(boolean enabled)
      Enable driving to the target pose.
      Parameters:
      enabled - Enable state of drive to pose.
      Returns:
      self.

    • translationHeadingOffset

      public SwerveInputStream translationHeadingOffset(BooleanSupplier enabled)
      Heading offset enabled boolean supplier.
      Parameters:
      enabled - Enable state
      Returns:
      self

    • translationHeadingOffset

      public SwerveInputStream translationHeadingOffset(boolean enabled)
      Heading offset enable
      Parameters:
      enabled - Enable state
      Returns:
      self

    • translationHeadingOffset

      public SwerveInputStream translationHeadingOffset(Rotation2d angle)
      Set the heading offset angle.
      Parameters:
      angle - Rotation2d offset to apply
      Returns:
      self

    • allianceRelativeControl

      public SwerveInputStream allianceRelativeControl(BooleanSupplier enabled)
      Modify the output ChassisSpeeds so that it is always relative to your alliance.
      Parameters:
      enabled - Alliance aware ChassisSpeeds output.
      Returns:
      self

    • allianceRelativeControl

      public SwerveInputStream allianceRelativeControl(boolean enabled)
      Modify the output ChassisSpeeds so that it is always relative to your alliance.
      Parameters:
      enabled - Alliance aware ChassisSpeeds output.
      Returns:
      self

    • cubeRotationControllerAxis

      public SwerveInputStream cubeRotationControllerAxis(BooleanSupplier enabled)
      Cube the angular velocity controller axis for a non-linear controls scheme.
      Parameters:
      enabled - Enabled state for the stream.
      Returns:
      self.

    • cubeRotationControllerAxis

      public SwerveInputStream cubeRotationControllerAxis(boolean enabled)
      Cube the angular velocity controller axis for a non-linear controls scheme.
      Parameters:
      enabled - Enabled state for the stream.
      Returns:
      self.

    • cubeTranslationControllerAxis

      public SwerveInputStream cubeTranslationControllerAxis(BooleanSupplier enabled)
      Cube the translation axis magnitude for a non-linear control scheme.
      Parameters:
      enabled - Enabled state for the stream
      Returns:
      self

    • cubeTranslationControllerAxis

      public SwerveInputStream cubeTranslationControllerAxis(boolean enabled)
      Cube the translation axis magnitude for a non-linear control scheme
      Parameters:
      enabled - Enabled state for the stream
      Returns:
      self

    • withControllerRotationAxis

      public SwerveInputStream withControllerRotationAxis(DoubleSupplier rot)
      Add a rotation axis for Angular Velocity control
      Parameters:
      rot - Rotation axis with values from [-1, 1]
      Returns:
      self

    • withControllerHeadingAxis

      public SwerveInputStream withControllerHeadingAxis(DoubleSupplier headingX, DoubleSupplier headingY)
      Add heading axis for Heading based control.
      Parameters:
      headingX - Heading X axis with values from [-1, 1]
      headingY - Heading Y axis with values from [-1, 1]
      Returns:
      self

    • deadband

      public SwerveInputStream deadband(double deadband)
      Set a deadband for all controller axis.
      Parameters:
      deadband - Deadband to set, should be between [0, 1)
      Returns:
      self

    • scaleTranslation

      public SwerveInputStream scaleTranslation(double scaleTranslation)
      Scale the translation axis for SwerveInputStream by a constant scalar value.
      Parameters:
      scaleTranslation - Translation axis scalar value. (0, 1]
      Returns:
      this

    • scaleRotation

      public SwerveInputStream scaleRotation(double scaleRotation)
      Scale the rotation axis input for SwerveInputStream to reduce the range in which they operate.
      Parameters:
      scaleRotation - Angular velocity axis scalar value. (0, 1]
      Returns:
      this

    • headingWhile

      public SwerveInputStream headingWhile(BooleanSupplier trigger)
      Output ChassisSpeeds based on heading while the supplier is True.
      Parameters:
      trigger - Supplier to use.
      Returns:
      this.

    • headingWhile

      public SwerveInputStream headingWhile(boolean headingState)
      Set the heading enable state.
      Parameters:
      headingState - Heading enabled state.
      Returns:
      this

    • aim

      public SwerveInputStream aim(Pose2d aimTarget)
      Aim the SwerveDrive at this pose while driving.
      Parameters:
      aimTarget - Pose2d to point at.
      Returns:
      this

    • aim

      public SwerveInputStream aim(Supplier<Pose2d> aimTarget)
      Aim the SwerveDrive at this pose while driving.
      Parameters:
      aimTarget - Supplier<Pose2d> to point at.
      Returns:
      this

    • aimLookahead

      public SwerveInputStream aimLookahead(Time lookaheadTime)
      Aim lookahead time for the SwerveDrive to estimate its current position while driving.

      Your camera takes a picture. It takes 10ms to process. It sends the data to the Rio/Control Hub (another 5ms). Your loop calculates the turret angle. You send the command. The turret motor takes time to accelerate.

      By the time the ball actually leaves the robot, you are in a different place than when you took the picture.

      The Fix: Project your position forward.

      double latencySeconds = CAMERA_LATENCY + MOTOR_LAG + SHOOTING_TIME;

      You will need to tune latencySeconds. It's often higher than you think (sometimes 100-200ms or more depending on the system).

      Parameters:
      lookaheadTime - Lookahead time for the SwerveDrive to estimate its current position.
      Returns:
      SwerveInputStream for chaining.

    • aimFeedforward

      public SwerveInputStream aimFeedforward(double kS, double kV, double kA)
      Aim feedforward for better tracking of the target.
      Parameters:
      kS - kS gain (voltage)
      kV - kV gain (radians/s/voltage)
      kA - kA gain (radians/s^2/voltage)
      Returns:
      SwerveInputStream for chaining.

    • aimWhile

      public SwerveInputStream aimWhile(BooleanSupplier trigger)
      Enable aiming while the trigger is true.
      Parameters:
      trigger - When True will enable aiming at the current target.
      Returns:
      this.

    • aimLock

      public Trigger aimLock(Angle tolerance)
      Aim is locked onto the target.
      Parameters:
      tolerance - Tolerance of the lock.
      Returns:
      Trigger for aim lock.

    • aimWhile

      public SwerveInputStream aimWhile(boolean trigger)
      Enable aiming while the trigger is true.
      Parameters:
      trigger - When True will enable aiming at the current target.
      Returns:
      this.

    • aimHeadingOffset

      public SwerveInputStream aimHeadingOffset(BooleanSupplier enabled)
      Aim heading offset enabled boolean supplier.
      Parameters:
      enabled - Boolean supplier to enable aim heading offset.
      Returns:
      this.

    • aimHeadingOffset

      public SwerveInputStream aimHeadingOffset(boolean enabled)
      Aim heading offset enabled
      Parameters:
      enabled - Boolean to enable aim heading offset.
      Returns:
      this.

    • aimHeadingOffset

      public SwerveInputStream aimHeadingOffset(Rotation2d offset)
      Set the aim heading offset.
      Parameters:
      offset - The offset applied to the aim heading target.
      Returns:
      this.

    • translationOnlyWhile

      public SwerveInputStream translationOnlyWhile(BooleanSupplier trigger)
      Enable locking of rotation and only translating, overrides everything.
      Parameters:
      trigger - Translation only while returns true.
      Returns:
      this

    • translationOnlyHeading

      public SwerveInputStream translationOnlyHeading(Angle heading)
      Set the translation only heading to be the provided heading.
      Parameters:
      heading - Heading to lock the translation to.
      Returns:
      SwerveInputStream for chaining.

    • translationOnlyWhile

      public SwerveInputStream translationOnlyWhile(boolean translationState)
      Enable locking of rotation and only translating, overrides everything.
      Parameters:
      translationState - Translation only if true.
      Returns:
      this

    • atTargetPose

      public boolean atTargetPose(double toleranceMeters)
      When the SwerveInputStream is in SwerveInputStream.SwerveInputMode.DRIVE_TO_POSE this function will return if the robot is at the desired pose within the defined tolerance.
      Parameters:
      toleranceMeters - Tolerance in meters.
      Returns:
      At target pose, true if current mode is not SwerveInputStream.SwerveInputMode.DRIVE_TO_POSE and no pose supplier has been given.

    • getTargetVector

      public Translation2d getTargetVector(Pose2d target)
      Get the target vector with a lookahead, if defined. Useful for shooting on the move implementations.

      NOTE: This is best when going in a straight line! Do not try to drive with a curve while doing this for the best results!

      Parameters:
      target - Target pose to
      Returns:
      Translation2d of the target vector.

    • calculateAngularVelocity

      public AngularVelocity calculateAngularVelocity(Angle target)
      Calculate the angular velocity required for the given target with the current heading, `controllerproperties.json` PID, and feedforward (if defined).
      Parameters:
      target - Target angle to calculate for.
      Returns:
      AngularVelocity to reach the target Angle.

    • get

      public ChassisSpeeds get()
      Gets a ChassisSpeeds
      Specified by:
      get in interface Supplier<ChassisSpeeds>
      Returns:
      ChassisSpeeds