Package com.marslib.swerve

Class SwerveSetpointGenerator

java.lang.Object

com.marslib.swerve.SwerveSetpointGenerator

public class SwerveSetpointGenerator
extends Object

Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path follower), and outputs a new setpoint that respects all of the kinematic constraints on module rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the robot will converge to the desired setpoint quickly while avoiding any intermediate state that is kinematically infeasible (and can result in wheel slip or robot heading drift as a result).

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
static class	SwerveSetpointGenerator.KinematicLimits
static class	SwerveSetpointGenerator.SwerveSetpoint

Constructor Summary

Constructors

Constructor	Description
SwerveSetpointGenerator(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics)

Method Summary

Modifier and Type	Method	Description
protected double	findDriveMaxS(double x0, double y0, double f0, double x1, double y1, double f1, double maxVelStep, int maxIterations)	Computes the maximum linear acceleration scalar \( s_{max} \) for the driving vector wheel slip threshold.
protected double	findSteeringMaxS(double x0, double y0, double f0, double x1, double y1, double f1, double maxDeviation, int maxIterations)	Computes the maximum positional scalar \( s_{max} \) for the steering rotation of a Swerve Module.
SwerveSetpointGenerator.SwerveSetpoint	generateSetpoint(SwerveSetpointGenerator.KinematicLimits limits, SwerveSetpointGenerator.SwerveSetpoint prevSetpoint, edu.wpi.first.math.kinematics.ChassisSpeeds desiredState, double dt)

Methods inherited from class java.lang.Object

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

Constructor Details

SwerveSetpointGenerator

public SwerveSetpointGenerator(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics)

Method Details

findSteeringMaxS

protected double findSteeringMaxS(double x0,
 double y0,
 double f0,
 double x1,
 double y1,
 double f1,
 double maxDeviation,
 int maxIterations)

Computes the maximum positional scalar \( s_{max} \) for the steering rotation of a Swerve Module.

Utilizes findRoot to determine the exact timestamp bound where the change in module orientation \( d\theta \) precisely equals the physical limit maxDeviation. The equation evaluates \( f(\theta) = \text{unwrap}(\theta) - \text{offset} = 0 \).

Parameters:

x0 - Prior structural X (Cosine mapping vector)

y0 - Prior structural Y (Sine mapping vector)

f0 - Previous rotation radians

x1 - Desired structural X

y1 - Desired structural Y

f1 - Desired rotation radians

maxDeviation - The structural Max Steering velocity * \( dt \)

maxIterations - Bisection cut-off depth.

Returns:

The maximum safe step multiplier scale.

findDriveMaxS

protected double findDriveMaxS(double x0,
 double y0,
 double f0,
 double x1,
 double y1,
 double f1,
 double maxVelStep,
 int maxIterations)

Computes the maximum linear acceleration scalar \( s_{max} \) for the driving vector wheel slip threshold.

Calculates the boundary threshold solving for: \( \sqrt{V_x^2 + V_y^2} - (\text{previous magnitude} + \text{max allowed acceleration step}) = 0 \).

Parameters:

x0 - Prior magnitude X velocity

y0 - Prior magnitude Y velocity

f0 - Previous hypotenuse sum velocity

x1 - Desired magnitude X velocity

y1 - Desired magnitude Y velocity

f1 - Desired hypotenuse sum velocity

maxVelStep - Absolute hardcap velocity slip threshold (\( \mu \cdot 9.81 \cdot dt \))

maxIterations - Limits Newton iteration overloop.

Returns:

A restricted translation scalar multiplier [0.0, 1.0].

generateSetpoint

public SwerveSetpointGenerator.SwerveSetpoint generateSetpoint(SwerveSetpointGenerator.KinematicLimits limits,
 SwerveSetpointGenerator.SwerveSetpoint prevSetpoint,
 edu.wpi.first.math.kinematics.ChassisSpeeds desiredState,
 double dt)