Skip to content

Python Example: Build an Auto

Michael Jansen edited this page Dec 7, 2023 · 1 revision

In PathPlannerLib, autos are created using the PathPlannerAuto class. This class is essentially just a command group that will build itself based on an auto file created in the GUI. Therefore, you can use PathPlannerAuto just like any other command, i.e add to a SendableChooser, bind to a button, etc.

Before creating an auto, you must first configure the AutoBuilder, so that the auto is able to build itself with the correct path following command and settings.

Configuring AutoBuilder

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 at the end of your drive subsystem's constructor. NOTE: You can only configure AutoBuilder once. An exception will be thrown if you try to configure it multiple times.

Holonomic

from pathplannerlib.auto import AutoBuilder
from pathplannerlib.config import HolonomicPathFollowerConfig, ReplanningConfig, PIDConstants

class SwerveSubsystem(Subsystem):
    def __init__(self):
        # Do all subsystem initialization here
        # ...

        # Configure the AutoBuilder last
        AutoBuilder.configureHolonomic(
            self.getPose, # Robot pose supplier
            self.resetPose, # Method to reset odometry (will be called if your auto has a starting pose)
            self.getRobotRelativeSpeeds, # ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
            self.driveRobotRelative, # Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
            HolonomicPathFollowerConfig( # HolonomicPathFollowerConfig, this should likely live in your Constants class
                PIDConstants(5.0, 0.0, 0.0), # Translation PID constants
                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.
                ReplanningConfig() # Default path replanning config. See the API for the options here
            ),
            self # Reference to this subsystem to set requirements
        )

Ramsete

This example will use the default Ramsete configuration.

from pathplannerlib.auto import AutoBuilder
from pathplannerlib.config import ReplanningConfig, PIDConstants

class DriveSubsystem(Subsystem):
    def __init__(self):
        # Do all subsystem initialization here
        # ...

        # Configure the AutoBuilder last
        AutoBuilder.configureRamsete(
            self.getPose, # Robot pose supplier
            self.resetPose, # Method to reset odometry (will be called if your auto has a starting pose)
            self.getCurrentSpeeds, # Current ChassisSpeeds supplier
            self.drive, # Method that will drive the robot given ChassisSpeeds
            ReplanningConfig(), # Default path replanning config. See the API for the options here
            self # Reference to this subsystem to set requirements
        )

LTV

from pathplannerlib.auto import AutoBuilder
from pathplannerlib.config import ReplanningConfig, PIDConstants

class DriveSubsystem(Subsystem):
    def __init__(self):
        # Do all subsystem initialization here
        # ...

        # Configure the AutoBuilder last
        AutoBuilder.configureLTV(
            self.getPose, # Robot pose supplier
            self.resetPose, # Method to reset odometry (will be called if your auto has a starting pose)
            self.getCurrentSpeeds, # Current ChassisSpeeds supplier
            self.drive, # Method that will drive the robot given ChassisSpeeds
            (0.0625, 0.125, 2.0), # qelems/error tolerances
            (1.0, 2.0), # relems/control effort
            0.02, # Robot control loop period in seconds. Default is 0.02
            ReplanningConfig(), # Default path replanning config. See the API for the options here
            self # Reference to this subsystem to set requirements
        )

Create 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.

from pathplannerlib.auto import PathPlannerAuto

class RobotContainer:
    def getAutonomousCommand():
        return PathPlannerAuto('Example Auto')
Clone this wiki locally