Package frc.robot.helpers

Class CustomSwerveInput

java.lang.Object
frc.robot.helpers.CustomSwerveInput
All Implemented Interfaces:
Supplier<edu.wpi.first.math.kinematics.ChassisSpeeds>
public class CustomSwerveInput extends Object implements Supplier<edu.wpi.first.math.kinematics.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);

 CustomSwerveInput driveAngularVelocity = CustomSwerveInput.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.

 CustomSwerveInput 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

    • CustomSwerveInput

      public CustomSwerveInput(swervelib.SwerveDrive drive, DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot)
      Create a CustomSwerveInput for an easy way to generate ChassisSpeeds from a driver controller.
      Parameters:
      drive - SwerveDrive object for transformation.
      x - Translation X input in range of [-1, 1]
      y - Translation Y input in range of [-1, 1]
      rot - Rotation input in range of [-1, 1]

    • CustomSwerveInput

      public CustomSwerveInput(swervelib.SwerveDrive drive, DoubleSupplier x, DoubleSupplier y, DoubleSupplier headingX, DoubleSupplier headingY)
      Create a CustomSwerveInput for an easy way to generate ChassisSpeeds from a driver controller.
      Parameters:
      drive - SwerveDrive object for transformation.
      x - Translation X input in range of [-1, 1]
      y - Translation Y input in range of [-1, 1]
      headingX - Heading X input in range of [-1, 1]
      headingY - Heading Y input in range of [-1, 1]

  • Method Details

    • of

      public static CustomSwerveInput of(swervelib.SwerveDrive drive, DoubleSupplier x, DoubleSupplier y)
      Create basic CustomSwerveInput 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:
      CustomSwerveInput to use as you see fit.

    • copy

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

    • robotRelative

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

    • robotRelative

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

    • driveToPose

      public CustomSwerveInput driveToPose(Supplier<edu.wpi.first.math.geometry.Pose2d> pose, edu.wpi.first.math.controller.ProfiledPIDController xPIDController, edu.wpi.first.math.controller.ProfiledPIDController yPIDController, edu.wpi.first.math.controller.ProfiledPIDController omegaPIDController)
      Drive to a given pose with the provided ProfiledPIDControllers
      Parameters:
      pose - Supplier<edu.wpi.first.math.geometry.Pose2d> for ease of use.
      xPIDController - PID controller for the X axis, units are m/s.
      yPIDController - PID controller for the Y axis, units are m/s.
      omegaPIDController - PID Controller for rotational axis, units are rad/s.
      Returns:
      self

    • driveToPose

      public CustomSwerveInput driveToPose(Supplier<edu.wpi.first.math.geometry.Pose2d> pose, edu.wpi.first.math.controller.ProfiledPIDController translation, edu.wpi.first.math.controller.ProfiledPIDController rotation)
      Drive to a given pose with the provided ProfiledPIDControllers
      Parameters:
      pose - Supplier<edu.wpi.first.math.geometry.Pose2d> for ease of use.
      translation - PID controller for the X and Y axis, units are m/s.
      rotation - PID Controller for rotational axis, units are rad/s.
      Returns:
      self

    • driveToPoseEnabled

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

    • driveToPoseEnabled

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

    • headingOffset

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

    • headingOffset

      public CustomSwerveInput headingOffset(boolean enabled)
      Heading offset enable
      Parameters:
      enabled - Enable state
      Returns:
      self

    • headingOffset

      public CustomSwerveInput headingOffset(edu.wpi.first.math.geometry.Rotation2d angle)
      Set the heading offset angle.
      Parameters:
      angle - Rotation2d offset to apply
      Returns:
      self

    • allianceRelativeControl

      public CustomSwerveInput 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 CustomSwerveInput 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 CustomSwerveInput 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 CustomSwerveInput 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 CustomSwerveInput 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 CustomSwerveInput 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 CustomSwerveInput withControllerRotationAxis(DoubleSupplier rot)
      Add a rotation axis for Angular Velocity control
      Parameters:
      rot - Rotation axis with values from [-1, 1]
      Returns:
      self

    • withControllerHeadingAxis

      public CustomSwerveInput 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

    • withHeading

      public CustomSwerveInput withHeading(Supplier<edu.wpi.first.math.geometry.Rotation2d> heading)
      Add direct heading supplier for Heading based control.
      Parameters:
      heading - Supplier that provides the desired heading as Rotation2d
      Returns:
      self

    • deadband

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

    • scaleTranslation

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

    • scaleTranslation

      public CustomSwerveInput scaleTranslation(BooleanSupplier condition, double scale)
      Scale the translation axis based on a boolean supplier. When the supplier returns true, the translation will be scaled by the provided scale; otherwise, it will remain unscaled.
      Parameters:
      condition - Boolean supplier determining if scaling should be applied.
      scale - Scale factor when condition is true.
      Returns:
      this

    • scaleRotation

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

    • headingWhile

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

    • headingWhile

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

    • aim

      public CustomSwerveInput aim(edu.wpi.first.math.geometry.Pose2d aimTarget)
      Aim the SwerveDrive at this pose while driving.
      Parameters:
      aimTarget - Pose2d to point at.
      Returns:
      this

    • aimWhile

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

    • aimWhile

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

    • translationOnlyWhile

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

    • translationOnlyWhile

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

    • get

      public edu.wpi.first.math.kinematics.ChassisSpeeds get()
      Gets a ChassisSpeeds
      Specified by:
      get in interface Supplier<edu.wpi.first.math.kinematics.ChassisSpeeds>
      Returns:
      ChassisSpeeds