Package xbot.common.subsystems.drive

Class SwerveKinematicsCalculator

java.lang.Object

xbot.common.subsystems.drive.SwerveKinematicsCalculator

public class SwerveKinematicsCalculator
extends Object

This class is designed to compute the motion of a swerve drive system from a starting position and velocity to a goal position and velocity. It takes inputs like acceleration, goal velocity, and max velocity to generate a sequence of motion stages that prioritizes the fastest possible route while respecting its goals. That said, it *may not* ALWAYS reach its goals if it is absurd/impossible!

NOTE: Quadratic formula code IS NOT ROBUST and is ONLY safe for SwerveSimpleTrajectoryCommand usages. Please make adjustments if you are planning to use the static method and note that the quadratic formula IRL may result in 0, 1, or 2 solutions; just that in this use case will guarantee 2 solutions.

UNITS-WISE: This calculator uses meters, but you can feed it any units!

Constructor Summary

Constructors

Constructor	Description
SwerveKinematicsCalculator(RobotAssertionManager assertionManager, Distance startPosition, Distance endPosition, SwervePointKinematics kinematics)

Method Summary

Modifier and Type	Method	Description
static Time	calculateTimeToGoalPosition(double acceleration, double initialVelocity, double initialPosition, double goalPosition)	Derived from formula (1D-Motion): (xf - xi) = (vi * time + 0.5 * a * time^2) From this, we'll get a 2nd degree polynomial that we'll use quadratic formula to solve for time
List<CalculatorNode>	generateNodeMap()
Distance	getDistanceTravelledAtCompletionPercentage(double percentage)	Returns the distance travelled when the operation have elapsed by percentage time
Distance	getDistanceTravelledAtTime(Time time)
List<CalculatorNode>	getNodeMap()
Distance	getTotalOperationDistance()
Time	getTotalOperationTime()
LinearVelocity	getVelocityAtDistanceTravelled(Distance distanceTravelled)
LinearVelocity	getVelocityAtFinish()

Methods inherited from class java.lang.Object

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

Constructor Details

SwerveKinematicsCalculator

public SwerveKinematicsCalculator(RobotAssertionManager assertionManager,
 Distance startPosition,
 Distance endPosition,
 SwervePointKinematics kinematics)

Method Details

calculateTimeToGoalPosition

public static Time calculateTimeToGoalPosition(double acceleration,
 double initialVelocity,
 double initialPosition,
 double goalPosition)

Derived from formula (1D-Motion): (xf - xi) = (vi * time + 0.5 * a * time^2) From this, we'll get a 2nd degree polynomial that we'll use quadratic formula to solve for time

Returns:

shortest time to get from initial position to goal position given acceleration and initial velocity. No constraints for velocity and acceleration will be taken into consideration.

generateNodeMap

public List<CalculatorNode> generateNodeMap()

getNodeMap

public List<CalculatorNode> getNodeMap()

getVelocityAtFinish

public LinearVelocity getVelocityAtFinish()

getTotalOperationTime

public Time getTotalOperationTime()

getDistanceTravelledAtTime

public Distance getDistanceTravelledAtTime(Time time)

getDistanceTravelledAtCompletionPercentage

public Distance getDistanceTravelledAtCompletionPercentage(double percentage)

Returns the distance travelled when the operation have elapsed by percentage time

Parameters:

percentage - progress of completion

Returns:

distance travelled in whatever units was used upon the instantiation of the calculator

getTotalOperationDistance

public Distance getTotalOperationDistance()

getVelocityAtDistanceTravelled

public LinearVelocity getVelocityAtDistanceTravelled(Distance distanceTravelled)