Package frc.robot.subsystems.swervedrive2.swervelib.math

## Class SwerveMath

- java.lang.Object
- - frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath

-     public class SwerveMath
      extends Object

  Mathematical functions which pertain to swerve drive.

- - ### Constructor Summary

    Constructors
    Constructor | Description
    --- | ---
    `SwerveMath()` |

  - ### Method Summary

    All Methods Static Methods Concrete Methods
    Modifier and Type | Method | Description
    --- | --- | ---
    `static double` | `applyDeadband​(double value, boolean scaled, double deadband)` | Algebraically apply a deadband using a piece wise function.
    `private static double` | `calcMaxAccel​(edu.wpi.first.math.geometry.Rotation2d angle, double chassisMass, double robotMass, edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)` | Calculates the maximum acceleration allowed in a direction without tipping the robot.
    `static double` | `calculateAngleKV​(double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)` | Calculate the angle kV which will be multiplied by the radians per second for the feedforward.
    `static double` | `calculateDegreesPerSteeringRotation​(double angleGearRatio, double pulsePerRotation)` | Calculate the degrees per steering rotation for the integrated encoder.
    `static double` | `calculateMaxAcceleration​(double cof)` | Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction.
    `static double` | `calculateMaxAcceleration​(double stallTorqueNm, double gearRatio, double moduleCount, double wheelDiameter, double robotMass)` | Calculate the maximum theoretical acceleration without friction.
    `static double` | `calculateMaxAngularVelocity​(double maxSpeed, double furthestModuleX, double furthestModuleY)` | Calculate the maximum angular velocity.
    `static double` | `calculateMetersPerRotation​(double wheelDiameter, double driveGearRatio, double pulsePerRotation)` | Calculate the meters per rotation for the integrated encoder.
    `static edu.wpi.first.math.geometry.Translation2d` | `limitVelocity​(edu.wpi.first.math.geometry.Translation2d commandedVelocity, edu.wpi.first.math.kinematics.ChassisSpeeds fieldVelocity, edu.wpi.first.math.geometry.Pose2d robotPose, double loopTime, double chassisMass, double robotMass, edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)` | Limits a commanded velocity to prevent exceeding the maximum acceleration given by `calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)`.

    - ### Methods inherited from class java.lang.Object

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

- - ### Constructor Detail

    - #### SwerveMath

          public SwerveMath()

  - ### Method Detail

    - #### calculateAngleKV

          public static double calculateAngleKV​(double optimalVoltage,
                                                double motorFreeSpeedRPM,
                                                double angleGearRatio)

      Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds / degree <=> (maxVolts) / (maxSpeed)
      Parameters:
        `optimalVoltage` - Optimal voltage to use when calculating the angle kV.
        `motorFreeSpeedRPM` - Motor free speed in Rotations per Minute.
        `angleGearRatio` - Angle gear ratio, the amount of times the motor as to turn for the wheel rotation.
      Returns:
        angle kV for feedforward.

    - #### calculateMetersPerRotation

          public static double calculateMetersPerRotation​(double wheelDiameter,
                                                          double driveGearRatio,
                                                          double pulsePerRotation)

      Calculate the meters per rotation for the integrated encoder. Calculation: 4in diameter wheels * pi [circumfrence] / gear ratio.
      Parameters:
        `wheelDiameter` - Wheel diameter in meters.
        `driveGearRatio` - The gear ratio of the drive motor.
        `pulsePerRotation` - The number of encoder pulses per rotation. 1 if using an integrated encoder.
      Returns:
        Meters per rotation for the drive motor.

    - #### applyDeadband

          public static double applyDeadband​(double value,
                                             boolean scaled,
                                             double deadband)

      Algebraically apply a deadband using a piece wise function.
      Parameters:
        `value` - value to apply deadband to.
        `scaled` - Use algebra to determine deadband by starting the value at 0 past deadband.
        `deadband` - The deadbnad to apply.
      Returns:
        Value with deadband applied.

    - #### calculateDegreesPerSteeringRotation

          public static double calculateDegreesPerSteeringRotation​(double angleGearRatio,
                                                                   double pulsePerRotation)

      Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts motor rotations to linear wheel distance and steering converts motor rotations to module azimuth.
      Parameters:
        `angleGearRatio` - The gear ratio of the steering motor.
        `pulsePerRotation` - The number of pulses in a complete rotation for the encoder, 1 if integrated.
      Returns:
        Degrees per steering rotation for the angle motor.

    - #### calculateMaxAngularVelocity

          public static double calculateMaxAngularVelocity​(double maxSpeed,
                                                           double furthestModuleX,
                                                           double furthestModuleY)

      Calculate the maximum angular velocity.
      Parameters:
        `maxSpeed` - Max speed of the robot in meters per second.
        `furthestModuleX` - X of the furthest module in meters.
        `furthestModuleY` - Y of the furthest module in meters.
      Returns:
        Maximum angular velocity in rad/s.

    - #### calculateMaxAcceleration

          public static double calculateMaxAcceleration​(double cof)

      Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction.
      Parameters:
        `cof` - Coefficient of Friction of the wheel grip tape.
      Returns:
        Practical maximum acceleration in m/s/s.

    - #### calculateMaxAcceleration

          public static double calculateMaxAcceleration​(double stallTorqueNm,
                                                        double gearRatio,
                                                        double moduleCount,
                                                        double wheelDiameter,
                                                        double robotMass)

      Calculate the maximum theoretical acceleration without friction.
      Parameters:
        `stallTorqueNm` - Stall torque of driving motor in nM.
        `gearRatio` - Gear ratio for driving motor number of motor rotations until one wheel rotation.
        `moduleCount` - Number of swerve modules.
        `wheelDiameter` - Wheel diameter in meters.
        `robotMass` - Mass of the robot in kg.
      Returns:
        Theoretical maximum acceleration in m/s/s.

    - #### calcMaxAccel

          private static double calcMaxAccel​(edu.wpi.first.math.geometry.Rotation2d angle,
                                             double chassisMass,
                                             double robotMass,
                                             edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity,
                                             SwerveDriveConfiguration config)

      Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from NetworkTables and is passed the direction in question.
      Parameters:
        `angle` - The direction in which to calculate max acceleration, as a Rotation2d. Note that this is robot-relative.
        `chassisMass` - Chassis mass in kg. (The weight of just the chassis not anything else)
        `robotMass` - The weight of the robot in kg. (Including manipulators, etc).
        `chassisCenterOfGravity` - Chassis center of gravity.
        `config` - The swerve drive configuration.
      Returns:
        Maximum acceleration allowed in the robot direction.

    - #### limitVelocity

          public static edu.wpi.first.math.geometry.Translation2d limitVelocity​(edu.wpi.first.math.geometry.Translation2d commandedVelocity,
                                                                                edu.wpi.first.math.kinematics.ChassisSpeeds fieldVelocity,
                                                                                edu.wpi.first.math.geometry.Pose2d robotPose,
                                                                                double loopTime,
                                                                                double chassisMass,
                                                                                double robotMass,
                                                                                edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity,
                                                                                SwerveDriveConfiguration config)

      Limits a commanded velocity to prevent exceeding the maximum acceleration given by `calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)`. Note that this takes and returns field-relative velocities.
      Parameters:
        `commandedVelocity` - The desired velocity
        `fieldVelocity` - The velocity of the robot within a field relative state.
        `robotPose` - The current pose of the robot.
        `loopTime` - The time it takes to update the velocity in seconds. Note: this should include the 100ms that it takes for a SparkMax velocity to update.
        `chassisMass` - Chassis mass in kg. (The weight of just the chassis not anything else)
        `robotMass` - The weight of the robot in kg. (Including manipulators, etc).
        `chassisCenterOfGravity` - Chassis center of gravity.
        `config` - The swerve drive configuration.
      Returns:
        The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable velocity.