Programming
Autonomous
PathPlanner
Building an Auton

Building An Auton

Download PathPlanner LIB

PathPlannerLib can be added to your robot code project using the "Install New Libraries (online)" feature in VSCode using the following JSON file URL:

https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json

Configure AutoBuilder

In PathPlannerLib, AutoBuilder is used to create full autonomous routines based on auto files created in the GUI app. In order for AutoBuilder to be able to build these auto routines, it must first be configured to control your robot.

There are a few options for configuring AutoBuilder, one for each type of path following command: Holonomic, Ramsete, and LTV.

ℹ️

Since all of the AutoBuilder configuration is related to the drive subsystem, it is recommended to configure AutoBuilder within your drive subsystem's constructor.

The following examples will assume that your drive subsystem has the following methods:

  • getPose - Returns the current robot pose as a Pose2d

  • resetPose - Resets the robot's odometry to the given pose

  • getRobotRelativeSpeeds or getCurrentSpeeds - Returns the current robot-relative ChassisSpeeds. This can be calculated using one of WPILib's drive kinematics classes

driveRobotRelative or drive - Outputs commands to the robot's drive motors given robot-relative ChassisSpeeds. This can be converted to module states or wheel speeds using WPILib's drive kinematics classes.

⚠️

The AutoBuilder configuration requires a method that will return true when a path should be flipped to the red side of the field. The origin of the field coordinate system will remain on the blue side.

If you wish to have any other alliance color based transformation, you must implement it yourself by changing the data passed to, and received from, PathPlannerLib's path following commands.

Holonomic (Swerve)

  public class DriveSubsystem extends SubsystemBase {
  public DriveSubsystem() {
    // All other subsystem initialization
    // ...
 
    // Load the RobotConfig from the GUI settings. You should probably
    // store this in your Constants file
    RobotConfig config;
    try{
      config = RobotConfig.fromGUISettings();
    } catch (Exception e) {
      // Handle exception as needed
      e.printStackTrace();
    }
 
    // Configure AutoBuilder last
    AutoBuilder.configure(
            this::getPose, // Robot pose supplier
            this::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
            this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
            (speeds, feedforwards) -> driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
            new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
                    new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
                    new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
            ),
            config, // The robot configuration
            () -> {
              // Boolean supplier that controls when the path will be mirrored for the red alliance
              // This will flip the path being followed to the red side of the field.
              // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
 
              var alliance = DriverStation.getAlliance();
              if (alliance.isPresent()) {
                return alliance.get() == DriverStation.Alliance.Red;
              }
              return false;
            },
            this // Reference to this subsystem to set requirements
    );
  }
}

LTV (Differential)

public class DriveSubsystem extends SubsystemBase {
  public DriveSubsystem() {
    // All other subsystem initialization
    // ...
 
    // Load the RobotConfig from the GUI settings. You should probably
    // store this in your Constants file
    RobotConfig config;
    try{
      config = RobotConfig.fromGUISettings();
    } catch (Exception e) {
      // Handle exception as needed
      e.printStackTrace();
    }
 
    // Configure AutoBuilder last
    AutoBuilder.configure(
            this::getPose, // Robot pose supplier
            this::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
            this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
            (speeds, feedforwards) -> driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
            new PPLTVController(0.02), // PPLTVController is the built in path following controller for differential drive trains
            config, // The robot configuration
            () -> {
              // Boolean supplier that controls when the path will be mirrored for the red alliance
              // This will flip the path being followed to the red side of the field.
              // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
 
              var alliance = DriverStation.getAlliance();
              if (alliance.isPresent()) {
                return alliance.get() == DriverStation.Alliance.Red;
              }
              return false;
            },
            this // Reference to this subsystem to set requirements
    );
  }
}

Load an Auto

After you have configured the AutoBuilder, creating an auto is as simple as constructing a PathPlannerAuto with the name of the auto you made in the GUI.

⚠️

It is highly recommended to create all of your autos when code starts, instead of creating them when you want to run them. Large delays can happen when loading complex autos/paths, so it is best to load them before they are needed.

In the interest of simplicity, this example will show an auto being loaded in the getAutonomousCommand function, which is called when auto is enabled. This is not the recommended way to load your autos.

public class RobotContainer {
  // ...
 
  public Command getAutonomousCommand() {
    return new PathPlannerAuto("Example Auto");
  }
}

Create a SendableChooser with all autos in project

After configuring the AutoBuilder, you have the option to build a SendableChooser that is automatically populated with every auto in the project.

⚠️

This method will load all autos in the deploy directory. Since the deploy process does not automatically clear the deploy directory, old auto files that have since been deleted from the project could remain on the RIO, therefore being added to the auto chooser.

To remove old options, the deploy directory will need to be cleared manually via SSH, WinSCP, reimaging the RIO, etc.

public class RobotContainer {
  private final SendableChooser<Command> autoChooser;
 
  public RobotContainer() {
    // ...
 
    // Build an auto chooser. This will use Commands.none() as the default option.
    autoChooser = AutoBuilder.buildAutoChooser();
 
    // Another option that allows you to specify the default auto by its name
    // autoChooser = AutoBuilder.buildAutoChooser("My Default Auto");
 
    SmartDashboard.putData("Auto Chooser", autoChooser);
  }
 
  public Command getAutonomousCommand() {
    return autoChooser.getSelected();
  }
}

Create a SendableChooser with certain autos in project

Note

ℹ️

This feature is only available in the Java version of PathPlannerLib

You can use the buildAutoChooserWithOptionsModifier method to process the autos before they are shown on shuffle board

⚠️

Be careful using runtime values when generating AutoChooser, as RobotContainer is built at robot code startup. Things like FMS values may not be present at startup

public class RobotContainer {
  private final SendableChooser<Command> autoChooser;
 
  public RobotContainer() {
    // ...
 
    // For convenience a programmer could change this when going to competition.
    boolean isCompetition = true;
 
    // Build an auto chooser. This will use Commands.none() as the default option.
    // As an example, this will only show autos that start with "comp" while at
    // competition as defined by the programmer
    autoChooser = AutoBuilder.buildAutoChooserWithOptionsModifier(
      (stream) -> isCompetition
        ? stream.filter(auto -> auto.getName().startsWith("comp"))
        : stream
    );
 
    SmartDashboard.putData("Auto Chooser", autoChooser);
  }
 
  public Command getAutonomousCommand() {
    return autoChooser.getSelected();
  }
}