Package org.carlmontrobotics.lib199.swerve

Class SwerveMath

java.lang.Object
org.carlmontrobotics.lib199.swerve.SwerveMath
public class SwerveMath extends Object
Class for SwerveMath - Team 199's custom implementation of methods for calculating the complicated mathematics involved in swerve driving, such as forward and inverse kinematics.

  • Constructor Summary

    Constructors
    Constructor
    Description
    SwerveMath()
     

  • Method Summary

    Modifier and Type
    Method
    Description
    static SwerveModuleState[]
    calculateSwerve(double forward, double strafe, double rotation, double length, double width)
    Robot-centric inverse kinematics: calculate SwerveModuleStates using desired forward, strafe, and rotation.
    static SwerveModuleState[]
    calculateSwerve(double forward, double strafe, double rotation, double heading, double length, double width)
    Field-centric inverse kinematics: calculate SwerveModuleStates using desired forward, strafe, and rotation.
    static double[]
    computeSetpoints(double normalizedSpeed, double angle, double encoderPosition, double gearRatio)
    Computes the setpoint values for speed and angle for a singular motor controller.
    static double
    convertAngle(double angle, double encoderPosition, double gearRatio)
    Converts the angle so that the robot can rotate in continuous circles.
    static double[]
    inverseSwerve(double length, double width, double heading, SwerveModuleState... states)
    Field-centric forward kinematics: calculate the required forward, strafe, and rotation values needed to create known SwerveModuleStates.
    static double[]
    inverseSwerve(double length, double width, SwerveModuleState... states)
    Robot-centric forward kinematics: calculate the required forward, strafe, and rotation values needed to create known SwerveModuleStates.
    static boolean
    shouldReverse(double angle, double encoderPosition, double gearRatio)
    Determines whether or not the robot should take the reverse direction to get to angle.

    Methods inherited from class java.lang.Object

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

  • Constructor Details

    • SwerveMath

      public SwerveMath()

  • Method Details

    • calculateSwerve

      public static SwerveModuleState[] calculateSwerve(double forward, double strafe, double rotation, double length, double width)
      Robot-centric inverse kinematics: calculate SwerveModuleStates using desired forward, strafe, and rotation. Inverse kinematics is essential in driving swerve drivetrain robots.
      Parameters:
      forward - The desired forward speed (in m/s) for the robot.
      strafe - The desired strafe speed (in m/s) for the robot.
      rotation - The desired rate of rotation (in rad/s) for the robot.
      length - The wheelbase (in m) of the robot.
      width - The trackwidth (in m) of the robot.
      Returns:
      An array consisting of four SwerveModuleState objects for the forward-left, forward-right, backward-left, and backward-right modules.

    • calculateSwerve

      public static SwerveModuleState[] calculateSwerve(double forward, double strafe, double rotation, double heading, double length, double width)
      Field-centric inverse kinematics: calculate SwerveModuleStates using desired forward, strafe, and rotation. Inverse kinematics is essential in driving swerve drivetrain robots.
      Parameters:
      forward - The desired forward speed (in m/s) for the robot.
      strafe - The desired strafe speed (in m/s) for the robot.
      rotation - The desired rate of rotation (in rad/s) for the robot.
      heading - The current heading of the robot (in rad) as measured by the robot's gyro.
      length - The wheelbase (in m) of the robot.
      width - The trackwidth (in m) of the robot.
      Returns:
      An array consisting of four SwerveModuleState objects for the forward-left, forward-right, backward-left, and backward-right modules.

    • inverseSwerve

      public static double[] inverseSwerve(double length, double width, SwerveModuleState... states)
      Robot-centric forward kinematics: calculate the required forward, strafe, and rotation values needed to create known SwerveModuleStates. Forward kinematics is useful in calculating the position of the robot and is used in odometry calculations.
      Parameters:
      length - The wheelbase (in m) of the robot.
      width - The trackwidth (in m) of the robot.
      states - Four SwerveModuleState objects in the order of forward-left, forward-right, backward-left, and backward-right.
      Returns:
      An array consisting of the the corresponding forward, strafe, and rotation values measured in m/s, m/s, and rad/s respectively.

    • inverseSwerve

      public static double[] inverseSwerve(double length, double width, double heading, SwerveModuleState... states)
      Field-centric forward kinematics: calculate the required forward, strafe, and rotation values needed to create known SwerveModuleStates. Forward kinematics is useful in calculating the position of the robot and is used in odometry calculations.
      Parameters:
      length - The wheelbase (in m) of the robot.
      width - The trackwidth (in m) of the robot.
      heading - The current heading (in rad) as measured by the robot's gyro.
      states - Four SwerveModuleState objects in the order of forward-left, forward-right, backward-left, and backward-right.
      Returns:
      An array consisting of the the corresponding forward, strafe, and rotation values measured in m/s, m/s, and rad/s respectively.

    • computeSetpoints

      public static double[] computeSetpoints(double normalizedSpeed, double angle, double encoderPosition, double gearRatio)
      Computes the setpoint values for speed and angle for a singular motor controller.
      Parameters:
      normalizedSpeed - The desired normalized speed, from -1.0 to 1.0.
      angle - The desired angle, from -1.0 to 1.0.
      encoderPosition - The position of the quadrature encoder on the turn motor controller.
      gearRatio - The gear ratio of the turn motor controller.
      Returns:
      An array of doubles containing the setpoint values in the order of speed then angle.

    • shouldReverse

      public static boolean shouldReverse(double angle, double encoderPosition, double gearRatio)
      Determines whether or not the robot should take the reverse direction to get to angle. e.g. if the robot was to turn 3π/2 radians clockwise, it would be better to turn π/2 radians counter-clockwsie. Credit to Team 100 for their code.
      Parameters:
      angle - The desired angle between -0.5 and 0.5
      encoderPosition - The position of the quadrature encoder on the turn motor controller.
      gearRatio - The gear ratio of the turn motor controller.
      Returns:
      A boolean representing whether the robot should reverse or not.

    • convertAngle

      public static double convertAngle(double angle, double encoderPosition, double gearRatio)
      Converts the angle so that the robot can rotate in continuous circles. Credit to Team 100 for their code.
      Parameters:
      angle - The desired angle from -1.0 to 1.0
      encoderPosition - The position of the quadrature encoder on the turn motor controller.
      gearRatio - The gear ratio of the turn motor controller.
      Returns:
      The converted angle between -0.5 and 0.5.