Skip to content

Java Example: Follow a Single Path

Michael Jansen edited this page Nov 14, 2023 · 5 revisions

Using AutoBuilder

The easiest way to create a command to follow a single path is by using AutoBuilder. You must have previously configured AutoBuilder in order to use this option.

See Java Example: Build an Auto

public Command getAutonomousCommand(){
    // Load the path you want to follow using its name in the GUI
    PathPlannerPath path = PathPlannerPath.fromPathFile("Example Path");

    // Create a path following command using AutoBuilder. This will also trigger event markers.
    return AutoBuilder.followPathWithEvents(path);
}

Manually Create Path Following Commands

Holonomic

// Assuming this is a method in your drive subsystem
public Command followPathCommand(String pathName){
    PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);

    // You must wrap the path following command in a FollowPathWithEvents command in order for event markers to work
    return new FollowPathWithEvents(
        new FollowPathHolonomic(
            path,
            this::getPose, // Robot pose supplier
            this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
            this::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
            new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
                new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
                new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants
                4.5, // Max module speed, in m/s
                0.4, // Drive base radius in meters. Distance from robot center to furthest module.
                new ReplanningConfig() // Default path replanning config. See the API for the options here
            ),
            this // Reference to this subsystem to set requirements
        ),
        path, // FollowPathWithEvents also requires the path
        this::getPose // FollowPathWithEvents also requires the robot pose supplier
    );
}

Ramsete

// Assuming this is a method in your drive subsystem
public Command followPathCommand(String pathName){
    PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);

    // You must wrap the path following command in a FollowPathWithEvents command in order for event markers to work
    return new FollowPathWithEvents(
        new FollowPathRamsete(
            path,
            this::getPose, // Robot pose supplier
            this::getCurrentSpeeds, // Current ChassisSpeeds supplier
            this::drive, // Method that will drive the robot given ChassisSpeeds
            new ReplanningConfig(), // Default path replanning config. See the API for the options here
            this // Reference to this subsystem to set requirements
        ),
        path, // FollowPathWithEvents also requires the path
        this::getPose // FollowPathWithEvents also requires the robot pose supplier
    );
}

LTV

// Assuming this is a method in your drive subsystem
public Command followPathCommand(String pathName){
    PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);

    // You must wrap the path following command in a FollowPathWithEvents command in order for event markers to work
    return new FollowPathWithEvents(
        new FollowPathLTV(
            path,
            this::getPose, // Robot pose supplier
            this::getCurrentSpeeds, // Current ChassisSpeeds supplier
            this::drive, // Method that will drive the robot given ChassisSpeeds
            0.02, // Robot control loop period in seconds. Default is 0.02
            new ReplanningConfig(), // Default path replanning config. See the API for the options here
            this // Reference to this subsystem to set requirements
        ),
        path, // FollowPathWithEvents also requires the path
        this::getPose // FollowPathWithEvents also requires the robot pose supplier
    );
}
Clone this wiki locally