Package swervelib

Class SwerveController

java.lang.Object
swervelib.SwerveController
public class SwerveController extends Object
Controller class used to convert raw inputs into robot speeds.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    edu.wpi.first.math.filter.SlewRateLimiter
    angleLimiter
    SlewRateLimiter for angular movement in radians/second.
    final SwerveControllerConfiguration
    config
    SwerveControllerConfiguration object storing data to generate the PIDController for controlling the robot heading, and deadband for heading joystick.
    double
    lastAngleScalar
    Last angle as a scalar [-1,1] the robot was set to.
    final edu.wpi.first.math.controller.PIDController
    thetaController
    PID Controller for the robot heading.
    edu.wpi.first.math.filter.SlewRateLimiter
    xLimiter
    SlewRateLimiter for movement in the X direction in meters/second.
    edu.wpi.first.math.filter.SlewRateLimiter
    yLimiter
    SlewRateLimiter for movement in the Y direction in meters/second.

  • Constructor Summary

    Constructors
    Constructor
    Description
    SwerveController(SwerveControllerConfiguration cfg)
    Construct the SwerveController object which is used for determining the speeds of the robot based on controller input.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addSlewRateLimiters(edu.wpi.first.math.filter.SlewRateLimiter x, edu.wpi.first.math.filter.SlewRateLimiter y, edu.wpi.first.math.filter.SlewRateLimiter angle)
    Add slew rate limiters to all controls.
    double
    getJoystickAngle(double headingX, double headingY)
    Get the angle in radians based off of the heading joysticks.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getRawTargetSpeeds(double xSpeed, double ySpeed, double omega)
    Get the ChassisSpeeds based of raw speeds desired in meters/second and heading in radians.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getRawTargetSpeeds(double xSpeed, double ySpeed, double targetHeadingAngleRadians, double currentHeadingAngleRadians)
    Get the ChassisSpeeds based of raw speeds desired in meters/second and heading in radians.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getTargetSpeeds(double xInput, double yInput, double angle, double currentHeadingAngleRadians, double maxSpeed)
    Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    getTargetSpeeds(double xInput, double yInput, double headingX, double headingY, double currentHeadingAngleRadians, double maxSpeed)
    Get the chassis speeds based on controller input of 2 joysticks.
    static edu.wpi.first.math.geometry.Translation2d
    getTranslation2d(edu.wpi.first.math.kinematics.ChassisSpeeds speeds)
    Helper function to get the Translation2d of the chassis speeds given the ChassisSpeeds.
    double
    headingCalculate(double currentHeadingAngleRadians, double targetHeadingAngleRadians)
    Calculate the angular velocity given the current and target heading angle in radians.
    void
    setMaximumAngularVelocity(double angularVelocity)
    Set a new maximum angular velocity that is different from the auto-generated one.
    boolean
    withinHypotDeadband(double x, double y)
    Calculate the hypot deadband and check if the joystick is within it.

    Methods inherited from class java.lang.Object

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

  • Field Details

    • config

      public final SwerveControllerConfiguration config
      SwerveControllerConfiguration object storing data to generate the PIDController for controlling the robot heading, and deadband for heading joystick.

    • thetaController

      public final edu.wpi.first.math.controller.PIDController thetaController
      PID Controller for the robot heading.

    • lastAngleScalar

      public double lastAngleScalar
      Last angle as a scalar [-1,1] the robot was set to.

    • xLimiter

      public edu.wpi.first.math.filter.SlewRateLimiter xLimiter
      SlewRateLimiter for movement in the X direction in meters/second.

    • yLimiter

      public edu.wpi.first.math.filter.SlewRateLimiter yLimiter
      SlewRateLimiter for movement in the Y direction in meters/second.

    • angleLimiter

      public edu.wpi.first.math.filter.SlewRateLimiter angleLimiter
      SlewRateLimiter for angular movement in radians/second.

  • Constructor Details

    • SwerveController

      public SwerveController(SwerveControllerConfiguration cfg)
      Construct the SwerveController object which is used for determining the speeds of the robot based on controller input.
      Parameters:
      cfg - SwerveControllerConfiguration containing the PIDF variables for the heading PIDF.

  • Method Details

    • getTranslation2d

      public static edu.wpi.first.math.geometry.Translation2d getTranslation2d(edu.wpi.first.math.kinematics.ChassisSpeeds speeds)
      Helper function to get the Translation2d of the chassis speeds given the ChassisSpeeds.
      Parameters:
      speeds - Chassis speeds.
      Returns:
      Translation2d of the speed the robot is going in.

    • addSlewRateLimiters

      public void addSlewRateLimiters(edu.wpi.first.math.filter.SlewRateLimiter x, edu.wpi.first.math.filter.SlewRateLimiter y, edu.wpi.first.math.filter.SlewRateLimiter angle)
      Add slew rate limiters to all controls. This prevents the robot from ramping up too much. To disable a SlewRateLimiter set the desired one to null.
      Parameters:
      x - The SlewRateLimiter for the X velocity in meters/second.
      y - The SlewRateLimiter for the Y velocity in meters/second.
      angle - The SlewRateLimiter for the angular velocity in radians/second.

    • withinHypotDeadband

      public boolean withinHypotDeadband(double x, double y)
      Calculate the hypot deadband and check if the joystick is within it.
      Parameters:
      x - The x value for the joystick in which the deadband should be applied.
      y - The y value for the joystick in which the deadband should be applied.
      Returns:
      Whether the values are within the deadband from SwerveControllerConfiguration.angleJoyStickRadiusDeadband.

    • getTargetSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double angle, double currentHeadingAngleRadians, double maxSpeed)
      Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle.
      Parameters:
      xInput - X joystick input for the robot to move in the X direction. X = xInput * maxSpeed
      yInput - Y joystick input for the robot to move in the Y direction. Y = yInput * maxSpeed;
      angle - The desired angle of the robot in radians.
      currentHeadingAngleRadians - The current robot heading in radians.
      maxSpeed - Maximum speed in meters per second.
      Returns:
      ChassisSpeeds which can be sent to th Swerve Drive.

    • getJoystickAngle

      public double getJoystickAngle(double headingX, double headingY)
      Get the angle in radians based off of the heading joysticks.
      Parameters:
      headingX - X joystick which controls the angle of the robot.
      headingY - Y joystick which controls the angle of the robot.
      Returns:
      angle in radians from the joystick.

    • getTargetSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY, double currentHeadingAngleRadians, double maxSpeed)
      Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for the angle of the robot.
      Parameters:
      xInput - X joystick input for the robot to move in the X direction.
      yInput - Y joystick input for the robot to move in the Y direction.
      headingX - X joystick which controls the angle of the robot.
      headingY - Y joystick which controls the angle of the robot.
      currentHeadingAngleRadians - The current robot heading in radians.
      maxSpeed - Maximum speed of the drive motors in meters per second, multiplier of the xInput and yInput.
      Returns:
      ChassisSpeeds which can be sent to th Swerve Drive.

    • getRawTargetSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double omega)
      Get the ChassisSpeeds based of raw speeds desired in meters/second and heading in radians.
      Parameters:
      xSpeed - X speed in meters per second.
      ySpeed - Y speed in meters per second.
      omega - Angular velocity in radians/second.
      Returns:
      ChassisSpeeds the robot should move to.

    • getRawTargetSpeeds

      public edu.wpi.first.math.kinematics.ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double targetHeadingAngleRadians, double currentHeadingAngleRadians)
      Get the ChassisSpeeds based of raw speeds desired in meters/second and heading in radians.
      Parameters:
      xSpeed - X speed in meters per second.
      ySpeed - Y speed in meters per second.
      targetHeadingAngleRadians - Target heading in radians.
      currentHeadingAngleRadians - Current heading in radians.
      Returns:
      ChassisSpeeds the robot should move to.

    • headingCalculate

      public double headingCalculate(double currentHeadingAngleRadians, double targetHeadingAngleRadians)
      Calculate the angular velocity given the current and target heading angle in radians.
      Parameters:
      currentHeadingAngleRadians - The current heading of the robot in radians.
      targetHeadingAngleRadians - The target heading of the robot in radians.
      Returns:
      Angular velocity in radians per second.

    • setMaximumAngularVelocity

      public void setMaximumAngularVelocity(double angularVelocity)
      Set a new maximum angular velocity that is different from the auto-generated one. Modified the SwerveControllerConfiguration.maxAngularVelocity field which is used in the SwerveController class for ChassisSpeeds generation.
      Parameters:
      angularVelocity - Angular velocity in radians per second.