Package frc.alotobots.library.subsystems.swervedrive.util

Class PathfindThenFollowPathWithDriveOverride

java.lang.Object

edu.wpi.first.wpilibj2.command.Command

frc.alotobots.library.subsystems.swervedrive.util.PathfindThenFollowPathWithDriveOverride

All Implemented Interfaces:

Sendable

public class PathfindThenFollowPathWithDriveOverride
extends Command

A command that will pathfind to the start of a path and then follow it with driver override capability. This extends PathPlanner's pathfinding functionality by allowing driver inputs to take control when needed and dynamically replanning the path when driver control is released.

Nested Class Summary

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

Command.InterruptionBehavior

Constructor Summary

Constructors

Constructor

Description

PathfindThenFollowPathWithDriveOverride(com.pathplanner.lib.path.PathPlannerPath goalPath, com.pathplanner.lib.path.PathConstraints constraints, Supplier<Pose2d> poseSupplier, Supplier<ChassisSpeeds> speedsSupplier, BiConsumer<ChassisSpeeds,com.pathplanner.lib.util.DriveFeedforwards> output, com.pathplanner.lib.controllers.PathFollowingController controller, com.pathplanner.lib.config.RobotConfig robotConfig, BooleanSupplier shouldFlipPath, Supplier<ChassisSpeeds> driverInput, double inputDeadband, boolean smoothTransition, double replanWaitTime, Subsystem... requirements)

Creates a new PathfindThenFollowPathWithDriveOverride command.

Method Summary

Modifier and Type	Method	Description
void	end(boolean interrupted)	Called once when the command ends or is interrupted.
void	execute()	Called repeatedly when this command is scheduled to run.
void	initialize()	Called when the command is initially scheduled.
boolean	isFinished()	Returns whether this command has finished.
boolean	isPathfinding()	Returns whether the command is currently in pathfinding mode.

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

Constructor Details

PathfindThenFollowPathWithDriveOverride

public PathfindThenFollowPathWithDriveOverride(com.pathplanner.lib.path.PathPlannerPath goalPath,
 com.pathplanner.lib.path.PathConstraints constraints,
 Supplier<Pose2d> poseSupplier,
 Supplier<ChassisSpeeds> speedsSupplier,
 BiConsumer<ChassisSpeeds,com.pathplanner.lib.util.DriveFeedforwards> output,
 com.pathplanner.lib.controllers.PathFollowingController controller,
 com.pathplanner.lib.config.RobotConfig robotConfig,
 BooleanSupplier shouldFlipPath,
 Supplier<ChassisSpeeds> driverInput,
 double inputDeadband,
 boolean smoothTransition,
 double replanWaitTime,
 Subsystem... requirements)

Creates a new PathfindThenFollowPathWithDriveOverride command.

Parameters:

goalPath - The path to follow

constraints - The path constraints to use when pathfinding

poseSupplier - A supplier for the robot's current pose

speedsSupplier - A supplier for the robot's current chassis speeds

output - The consumer for the output chassis speeds

controller - The path following controller to use

robotConfig - The robot configuration

shouldFlipPath - Whether the path should be flipped (e.g., for different alliances)

driverInput - A supplier for driver input as chassis speeds

inputDeadband - The deadband to apply to driver inputs

smoothTransition - Whether to smoothly transition between autonomous and driver control

replanWaitTime - Time to wait before replanning after driver input stops

requirements - The subsystems required by this command

Method Details

initialize

public void initialize()

Called when the command is initially scheduled. Initializes timing mechanisms and starts pathfinding.

Overrides:

initialize in class Command

execute

public void execute()

Called repeatedly when this command is scheduled to run. Handles transitions between autonomous path following and driver control.

Overrides:

execute in class Command

end

public void end(boolean interrupted)

Called once when the command ends or is interrupted. Ensures proper cleanup of resources and commands.

Overrides:

end in class Command

Parameters:

interrupted - Whether the command was interrupted

isFinished

public boolean isFinished()

Returns whether this command has finished.

Overrides:

isFinished in class Command

Returns:

True if the command is finished

isPathfinding

public boolean isPathfinding()

Returns whether the command is currently in pathfinding mode.

Returns:

True if actively pathfinding, false if in driver control