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 Details

    • SwerveKinematicsCalculator

      public SwerveKinematicsCalculator(RobotAssertionManager assertionManager, org.wpilib.units.measure.Distance startPosition, org.wpilib.units.measure.Distance endPosition, SwervePointKinematics kinematics)

  • Method Details

    • calculateTimeToGoalPosition

      public static org.wpilib.units.measure.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 org.wpilib.units.measure.LinearVelocity getVelocityAtFinish()

    • getTotalOperationTime

      public org.wpilib.units.measure.Time getTotalOperationTime()

    • getDistanceTravelledAtTime

      public org.wpilib.units.measure.Distance getDistanceTravelledAtTime(org.wpilib.units.measure.Time time)

    • getDistanceTravelledAtCompletionPercentage

      public org.wpilib.units.measure.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 org.wpilib.units.measure.Distance getTotalOperationDistance()

    • getVelocityAtDistanceTravelled

      public org.wpilib.units.measure.LinearVelocity getVelocityAtDistanceTravelled(org.wpilib.units.measure.Distance distanceTravelled)