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.
static enum	FollowPath.RotationOverrideBehavior	Determines how a rotation override interacts with BLine's normal constraints.

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
static void	clearRotationOverride()	Clears the active rotation override, restoring normal path rotation control.
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	overrideRotation(DoubleSupplier supplier)	Overrides the rotational output of all FollowPath commands.
static void	overrideRotation(DoubleSupplier supplier, FollowPath.RotationOverrideBehavior behavior)	Overrides the rotational output of all FollowPath commands.
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.

The key must match the lib_key of an Path.EventTrigger in a path JSON file. The action runs inline from this FollowPath command when the trigger's t_ratio is reached, so it should be quick and non-blocking. Use registerEventTrigger(String, Command) when the trigger should start a normal WPILib command.

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.

The key must match the lib_key of an Path.EventTrigger in a path JSON file. When the marker is reached, BLine schedules the supplied command with WPILib's CommandScheduler. The command is not automatically proxied here and is not automatically canceled when the path ends; it runs until it finishes or is interrupted by normal WPILib scheduling rules.

If this event command requires a subsystem that is also used by commands before or after the path, compose the surrounding autonomous routine with BLineCommands so the outer composition does not hold those child requirements for its whole lifetime.

Parameters:

key - The event trigger key referenced in JSON

command - The command to schedule when the trigger is reached

overrideRotation

public static void overrideRotation(DoubleSupplier supplier)

Overrides the rotational output of all FollowPath commands.

The supplier is called every execution cycle while active and must return omega in radians per second. This overload bypasses BLine's rotational velocity and acceleration limits so the caller owns the final path-follower omega command.

Call clearRotationOverride() when the override should stop affecting path following.

Parameters:

supplier - supplies omega in radians per second

Throws:

IllegalArgumentException - if supplier is null

overrideRotation

public static void overrideRotation(DoubleSupplier supplier,
 FollowPath.RotationOverrideBehavior behavior)

Overrides the rotational output of all FollowPath commands.

The supplier is called every execution cycle while active and must return omega in radians per second. Use FollowPath.RotationOverrideBehavior.RESPECT_CONSTRAINTS to keep BLine's normal rotational limits, or FollowPath.RotationOverrideBehavior.BYPASS_CONSTRAINTS when the caller owns the final rotational command.

Call clearRotationOverride() when the override should stop affecting path following.

Parameters:

supplier - supplies omega in radians per second

behavior - whether the supplied omega should respect or bypass BLine constraints

Throws:

IllegalArgumentException - if supplier or behavior is null

clearRotationOverride

public static void clearRotationOverride()

Clears the active rotation override, restoring normal path rotation control.

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