Package frc.robot.lib.BLine

Class FollowPath

java.lang.Object

edu.wpi.first.wpilibj2.command.Command

frc.robot.lib.BLine.FollowPath

All Implemented Interfaces:

edu.wpi.first.util.sendable.Sendable

public class FollowPath
extends edu.wpi.first.wpilibj2.command.Command

A WPILib Command that follows a Path using PID controllers for translation and rotation.

This command drives the robot along a defined path by tracking translation targets sequentially while simultaneously managing rotation targets. The command uses three PID controllers:

• Translation Controller: Calculates command speed by minimizing total path distance remaining
• Rotation Controller: Controls holonomic rotation toward the current rotation target
• Cross-Track Controller: Minimizes deviation from the line between waypoints

The path following algorithm works by:

1. Calculating command robot speed via a PID controller minimizing total path distance remaining
2. Determining velocity direction by pointing toward the current translation target
3. Advancing to the next translation target when within the handoff radius of the current one
4. Applying cross-track correction to stay on the line between waypoints
5. Interpolating rotation based on progress between rotation targets
6. Applying rate limiting via ChassisRateLimiter to respect constraints

Usage

Use the FollowPath.Builder class to construct FollowPath commands:

 FollowPath.Builder pathBuilder = new FollowPath.Builder(
     driveSubsystem,
     this::getPose,
     this::getRobotRelativeSpeeds,
     this::driveRobotRelative,
     new PIDController(5.0, 0, 0),  // translation
     new PIDController(3.0, 0, 0),  // rotation
     new PIDController(2.0, 0, 0)   // cross-track
 ).withDefaultShouldFlip()
  .withPoseReset(this::resetPose);
 
 // Then build commands for specific paths:
 Command followAuto = pathBuilder.build(new Path("myPath"));
 

Logging

The command supports optional logging via consumer functions. Set up logging callbacks using:

• setPoseLoggingConsumer(Consumer) - Log pose data
• setDoubleLoggingConsumer(Consumer) - Log numeric values
• setBooleanLoggingConsumer(Consumer) - Log boolean states
• setTranslationListLoggingConsumer(Consumer) - Log translation arrays

See Also:

• Path
• FollowPath.Builder
• ChassisRateLimiter

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
static class	FollowPath.Builder	Builder class for constructing FollowPath commands with a fluent API.

Nested classes/interfaces inherited from class edu.wpi.first.wpilibj2.command.Command

edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior

Method Summary

Modifier and Type	Method	Description
void	end(boolean interrupted)
void	execute()
int	getCurrentRotationElementIndex()	Gets the current rotation element index in the path.
int	getCurrentTranslationElementIndex()	Gets the current translation element index in the path.
double	getRemainingPathDistanceMeters()	Gets the estimated remaining path distance from the robot's current position.
void	initialize()
boolean	isFinished()
static void	registerEventTrigger(String key, edu.wpi.first.wpilibj2.command.Command command)	Registers an event trigger action by key using a WPILib Command.
static void	registerEventTrigger(String key, Runnable action)	Registers an event trigger action by key.
static void	setBooleanLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,Boolean>> booleanLoggingConsumer)	Sets the consumer for logging boolean values during path following.
static void	setDoubleLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,Double>> doubleLoggingConsumer)	Sets the consumer for logging numeric values during path following.
static void	setPoseLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,edu.wpi.first.math.geometry.Pose2d>> poseLoggingConsumer)	Sets the consumer for logging pose data during path following.
static void	setTranslationListLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,edu.wpi.first.math.geometry.Translation2d[]>> translationListLoggingConsumer)	Sets the consumer for logging translation arrays during path following.

Methods inherited from class edu.wpi.first.wpilibj2.command.Command

addRequirements, addRequirements, alongWith, andThen, andThen, asProxy, beforeStarting, beforeStarting, cancel, deadlineFor, deadlineWith, finallyDo, finallyDo, getInterruptionBehavior, getName, getRequirements, getSubsystem, handleInterrupt, hasRequirement, ignoringDisable, initSendable, isScheduled, onlyIf, onlyWhile, raceWith, repeatedly, runsWhenDisabled, schedule, setName, setSubsystem, unless, until, withDeadline, withInterruptBehavior, withName, withTimeout, withTimeout

Methods inherited from class java.lang.Object

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

Method Details

registerEventTrigger

public static void registerEventTrigger(String key,
 Runnable action)

Registers an event trigger action by key.

Parameters:

key - The event trigger key referenced in JSON

action - The action to execute when the trigger is reached

registerEventTrigger

public static void registerEventTrigger(String key,
 edu.wpi.first.wpilibj2.command.Command command)

Registers an event trigger action by key using a WPILib Command.

Parameters:

key - The event trigger key referenced in JSON

command - The command to schedule when the trigger is reached

setPoseLoggingConsumer

public static void setPoseLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,edu.wpi.first.math.geometry.Pose2d>> poseLoggingConsumer)

Sets the consumer for logging pose data during path following.

The consumer receives pairs of (key, Pose2d) for various internal poses such as closest points on path segments.

Parameters:

poseLoggingConsumer - The consumer to receive pose logging data, or null to disable

setTranslationListLoggingConsumer

public static void setTranslationListLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,edu.wpi.first.math.geometry.Translation2d[]>> translationListLoggingConsumer)

Sets the consumer for logging translation arrays during path following.

The consumer receives pairs of (key, Translation2d[]) for data such as path waypoints and robot position history.

Parameters:

translationListLoggingConsumer - The consumer to receive translation list data, or null to disable

setBooleanLoggingConsumer

public static void setBooleanLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,Boolean>> booleanLoggingConsumer)

Sets the consumer for logging boolean values during path following.

The consumer receives pairs of (key, Boolean) for state flags such as completion status.

Parameters:

booleanLoggingConsumer - The consumer to receive boolean logging data, or null to disable

setDoubleLoggingConsumer

public static void setDoubleLoggingConsumer(Consumer<edu.wpi.first.math.Pair<String,Double>> doubleLoggingConsumer)

Sets the consumer for logging numeric values during path following.

The consumer receives pairs of (key, Double) for various metrics such as remaining distance, controller outputs, and target indices.

Parameters:

doubleLoggingConsumer - The consumer to receive double logging data, or null to disable

initialize

public void initialize()

Overrides:

initialize in class edu.wpi.first.wpilibj2.command.Command

execute

public void execute()

Overrides:

execute in class edu.wpi.first.wpilibj2.command.Command

isFinished

public boolean isFinished()

Overrides:

isFinished in class edu.wpi.first.wpilibj2.command.Command

end

public void end(boolean interrupted)

Overrides:

end in class edu.wpi.first.wpilibj2.command.Command

getCurrentRotationElementIndex

public int getCurrentRotationElementIndex()

Gets the current rotation element index in the path.

This index represents the current rotation target being tracked, where both waypoint rotations and standalone rotation targets are counted together.

Returns:

The current rotation element index (0-based), or -1 when no active rotation target remains

getCurrentTranslationElementIndex

public int getCurrentTranslationElementIndex()

Gets the current translation element index in the path.

This index represents the current translation target being tracked, where both waypoint translations and standalone translation targets are counted together.

Returns:

The current translation element index (0-based)

getRemainingPathDistanceMeters

public double getRemainingPathDistanceMeters()

Gets the estimated remaining path distance from the robot's current position.

This value is computed live from the command's current traversal cursor and translation targets. It mirrors the distance basis used by the translation controller during execution.

Returns 0.0 when the command is not in a valid traversal state (for example, invalid path or uninitialized/invalid translation cursor).

Returns:

Remaining path distance in meters