PathPlannerLib
Loading...
Searching...
No Matches
pathplanner::SwerveSetpointGenerator Class Reference

#include <SwerveSetpointGenerator.h>

Public Member Functions

 SwerveSetpointGenerator ()
 
 SwerveSetpointGenerator (const RobotConfig &config, units::turns_per_second_t maxSteerVelocity)
 
SwerveSetpoint generateSetpoint (SwerveSetpoint prevSetpoint, frc::ChassisSpeeds desiredStateRobotRelative, std::optional< PathConstraints > constraints, units::second_t dt, units::volt_t inputVoltage)
 
SwerveSetpoint generateSetpoint (SwerveSetpoint prevSetpoint, frc::ChassisSpeeds desiredStateRobotRelative, std::optional< PathConstraints > constraints, units::second_t dt)
 
SwerveSetpoint generateSetpoint (SwerveSetpoint prevSetpoint, frc::ChassisSpeeds desiredStateRobotRelative, units::second_t dt, units::volt_t inputVoltage)
 
SwerveSetpoint generateSetpoint (SwerveSetpoint prevSetpoint, frc::ChassisSpeeds desiredStateRobotRelative, units::second_t dt)
 

Static Public Member Functions

static bool flipHeading (frc::Rotation2d prevToGoal)
 
static units::radian_t unwrapAngle (double ref, double angle)
 

Detailed Description

Swerve setpoint generator based on a version created by FRC team 254.

Takes a prior setpoint, a desired setpoint, and outputs a new setpoint that respects all the kinematic constraints on module rotation and wheel velocity/torque, as well as preventing any forces acting on a module's wheel from exceeding the force of friction.

Constructor & Destructor Documentation

◆ SwerveSetpointGenerator() [1/2]

SwerveSetpointGenerator::SwerveSetpointGenerator ( )

Create a new swerve setpoint generator

◆ SwerveSetpointGenerator() [2/2]

SwerveSetpointGenerator::SwerveSetpointGenerator ( const RobotConfig config,
units::turns_per_second_t  maxSteerVelocity 
)

Create a new swerve setpoint generator

Parameters
configThe robot configuration
maxSteerVelocityThe maximum rotation velocity of a swerve module, in turns per second

Member Function Documentation

◆ flipHeading()

static bool pathplanner::SwerveSetpointGenerator::flipHeading ( frc::Rotation2d  prevToGoal)
inlinestatic

Check if it would be faster to go to the opposite of the goal heading (and reverse drive direction).

Parameters
prevToGoalThe rotation from the previous state to the goal state (i.e. prev.inverse().rotateBy(goal)).
Returns
True if the shortest path to achieve this rotation involves flipping the drive direction.

◆ generateSetpoint() [1/4]

SwerveSetpoint SwerveSetpointGenerator::generateSetpoint ( SwerveSetpoint  prevSetpoint,
frc::ChassisSpeeds  desiredStateRobotRelative,
std::optional< PathConstraints constraints,
units::second_t  dt 
)

Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from this method. This method will discretize the speeds for you.

Note: This method will automatically use the current robot controller input voltage.

Parameters
prevSetpointThe previous setpoint motion. Normally, you'd pass in the previous iteration setpoint instead of the actual measured/estimated kinematic state.
desiredStateRobotRelativeThe desired state of motion, such as from the driver sticks or a path following algorithm.
constraintsThe arbitrary constraints to respect along with the robot's max capabilities. If this is nullopt, the generator will only limit setpoints by the robot's max capabilities.
dtThe loop time.
Returns
A Setpoint object that satisfies all the kinematic/friction limits while converging to desiredState quickly.

◆ generateSetpoint() [2/4]

SwerveSetpoint SwerveSetpointGenerator::generateSetpoint ( SwerveSetpoint  prevSetpoint,
frc::ChassisSpeeds  desiredStateRobotRelative,
std::optional< PathConstraints constraints,
units::second_t  dt,
units::volt_t  inputVoltage 
)

Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds passed into or returned from this method. This method will discretize the speeds for you.

Parameters
prevSetpointThe previous setpoint motion. Normally, you'd pass in the previous iteration setpoint instead of the actual measured/estimated kinematic state.
desiredStateRobotRelativeThe desired state of motion, such as from the driver sticks or a path following algorithm.
constraintsThe arbitrary constraints to respect along with the robot's max capabilities. If this is nullopt, the generator will only limit setpoints by the robot's max capabilities.
dtThe loop time.
inputVoltageThe input voltage of the drive motor controllers, in volts. This can also be a static nominal voltage if you do not want the setpoint generator to react to changes in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input voltage will be clamped to a minimum of the robot controller's brownout voltage.
Returns
A Setpoint object that satisfies all the kinematic/friction limits while converging to desiredState quickly.

◆ generateSetpoint() [3/4]

SwerveSetpoint pathplanner::SwerveSetpointGenerator::generateSetpoint ( SwerveSetpoint  prevSetpoint,
frc::ChassisSpeeds  desiredStateRobotRelative,
units::second_t  dt 
)
inline

Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from this method. This method will discretize the speeds for you.

Note: This method will automatically use the current robot controller input voltage.

Parameters
prevSetpointThe previous setpoint motion. Normally, you'd pass in the previous iteration setpoint instead of the actual measured/estimated kinematic state.
desiredStateRobotRelativeThe desired state of motion, such as from the driver sticks or a path following algorithm.
dtThe loop time.
Returns
A Setpoint object that satisfies all the kinematic/friction limits while converging to desiredState quickly.

◆ generateSetpoint() [4/4]

SwerveSetpoint pathplanner::SwerveSetpointGenerator::generateSetpoint ( SwerveSetpoint  prevSetpoint,
frc::ChassisSpeeds  desiredStateRobotRelative,
units::second_t  dt,
units::volt_t  inputVoltage 
)
inline

Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds passed into or returned from this method. This method will discretize the speeds for you.

Parameters
prevSetpointThe previous setpoint motion. Normally, you'd pass in the previous iteration setpoint instead of the actual measured/estimated kinematic state.
desiredStateRobotRelativeThe desired state of motion, such as from the driver sticks or a path following algorithm.
dtThe loop time.
inputVoltageThe input voltage of the drive motor controllers, in volts. This can also be a static nominal voltage if you do not want the setpoint generator to react to changes in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input voltage will be clamped to a minimum of the robot controller's brownout voltage.
Returns
A Setpoint object that satisfies all the kinematic/friction limits while converging to desiredState quickly.

The documentation for this class was generated from the following files: