# SwerveMath
declaration: package: swervelib.math, class: SwerveMath

Package swervelib.math

# Class SwerveMath

java.lang.Object
swervelib.math.SwerveMath

public class SwerveMath extends Object
Mathematical functions which pertain to swerve drive.

## Constructor Summary

Constructors
Constructor
Description
`SwerveMath()`

## Method Summary

Modifier and Type
Method
Description
`static void`
`antiJitter(edu.wpi.first.math.kinematics.SwerveModuleState moduleState, edu.wpi.first.math.kinematics.SwerveModuleState lastModuleState, double maxSpeed)`
Perform anti-jitter within modules if the speed requested is too low.
`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, List<Matter> matter, double robotMass, SwerveDriveConfiguration config)`
Calculates the maximum acceleration allowed in a direction without tipping the robot.
`static double`
`calculateDegreesPerSteeringRotation(double angleGearRatio)`
Calculate the degrees per steering rotation for the integrated encoder.
`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)`
Calculate the meters per rotation for the integrated encoder.
`static double`
`calculateMetersPerRotation(double wheelDiameter, double driveGearRatio, double pulsePerRotation)`
Calculate the meters per rotation for the integrated encoder.
`static edu.wpi.first.math.controller.SimpleMotorFeedforward`
`createDriveFeedforward(double optimalVoltage, double maxSpeed, double wheelGripCoefficientOfFriction)`
Create the drive feedforward for swerve modules.
`static SwerveModuleConfiguration`
`getSwerveModule(SwerveModule[] modules, boolean front, boolean left)`
Get the fruthest module from center based on the module locations.
`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 robotMass, List<Matter> matter, SwerveDriveConfiguration config)`
Limits a commanded velocity to prevent exceeding the maximum acceleration given by `calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d, java.util.List<swervelib.math.Matter>, double, swervelib.parser.SwerveDriveConfiguration)`.
`static double`
`normalizeAngle(double angle)`
Normalize an angle to be within 0 to 360.
`static double`
`placeInAppropriate0To360Scope(double scopeReference, double newAngle)`
Put an angle within the 360 deg scope of a reference.
`static edu.wpi.first.math.geometry.Twist2d`
`PoseLog(edu.wpi.first.math.geometry.Pose2d transform)`
Logical inverse of the Pose exponential from 254.

### 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

### calculateMetersPerRotation

public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio, double pulsePerRotation)
Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER RESOLUTION)

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.

### calculateMetersPerRotation

public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio)
Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO)

Parameters:
  `wheelDiameter` - Wheel diameter in meters.
  `driveGearRatio` - The gear ratio of the drive motor.
Returns:
  Meters per rotation for the drive motor.

### normalizeAngle

public static double normalizeAngle(double angle)
Normalize an angle to be within 0 to 360.

Parameters:
  `angle` - Angle in degrees.
Returns:
  Normalized angle in degrees.

### 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.

### createDriveFeedforward

public static edu.wpi.first.math.controller.SimpleMotorFeedforward createDriveFeedforward(double optimalVoltage, double maxSpeed, double wheelGripCoefficientOfFriction)
Create the drive feedforward for swerve modules.

Parameters:
  `optimalVoltage` - Optimal voltage to calculate kV (voltage/max Velocity)
  `maxSpeed` - Maximum velocity in meters per second to use for the feed forward, should be as close to physical max as possible.
  `wheelGripCoefficientOfFriction` - Wheel grip coefficient of friction for kA (voltage/(cof*9.81))
Returns:
  Drive feedforward for drive motor on a swerve module.

### 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.

### calculateDegreesPerSteeringRotation

public static double calculateDegreesPerSteeringRotation(double angleGearRatio)
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.
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, List<Matter> matter, double robotMass, 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.
  `matter` - Matter that the robot is composed of in kg. (Includes chassis)
  `robotMass` - The weight of the robot in kg. (Including manipulators, etc).
  `config` - The swerve drive configuration.
Returns:
  Maximum acceleration allowed in the robot direction.

### PoseLog

public static edu.wpi.first.math.geometry.Twist2d PoseLog(edu.wpi.first.math.geometry.Pose2d transform)
Logical inverse of the Pose exponential from 254. Taken from team 3181.

Parameters:
  `transform` - Pose to perform the log on.
Returns:
  `Twist2d` of the transformed pose.

### 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 robotMass, List<Matter> matter, SwerveDriveConfiguration config)
Limits a commanded velocity to prevent exceeding the maximum acceleration given by `calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d, java.util.List<swervelib.math.Matter>, double, swervelib.parser.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.
  `matter` - Matter that the robot is composed of with position in meters and mass in kg.
  `robotMass` - The weight of the robot in kg. (Including manipulators, etc).
  `config` - The swerve drive configuration.
Returns:
  The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable velocity.

### getSwerveModule

public static SwerveModuleConfiguration getSwerveModule(SwerveModule[] modules, boolean front, boolean left)
Get the fruthest module from center based on the module locations.

Parameters:
  `modules` - Swerve module list.
  `front` - True = furthest front, False = furthest back.
  `left` - True = furthest left, False = furthest right.
Returns:
  Module location which is the furthest from center and abides by parameters.

### placeInAppropriate0To360Scope

public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle)
Put an angle within the 360 deg scope of a reference. For example, given a scope reference of 756 degrees, assumes the full scope is (720-1080), and places an angle of 22 degrees into it, returning 742 deg.

Parameters:
  `scopeReference` - Current Angle (deg)
  `newAngle` - Target Angle (deg)
Returns:
  Closest angle within scope (deg)

### antiJitter

public static void antiJitter(edu.wpi.first.math.kinematics.SwerveModuleState moduleState, edu.wpi.first.math.kinematics.SwerveModuleState lastModuleState, double maxSpeed)
Perform anti-jitter within modules if the speed requested is too low.

Parameters:
  `moduleState` - Current `SwerveModuleState` requested.
  `lastModuleState` - Previous `SwerveModuleState` used.
  `maxSpeed` - Maximum speed of the modules.