Cpp Example: Build an Auto - mjansen4857/pathplanner GitHub Wiki
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
#include <pathplanner/lib/auto/AutoBuilder.h>
#include <pathplanner/lib/util/HolonomicPathFollowerConfig.h>
#include <pathplanner/lib/util/PIDConstants.h>
#include <pathplanner/lib/util/ReplanningConfig.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
using namespace pathplanner;
SwerveSubsystem::SwerveSubsystem(){
// Do all subsystem initialization here
// ...
// Configure the AutoBuilder last
AutoBuilder::configureHolonomic(
[this](){ return getPose(); }, // Robot pose supplier
[this](frc::Pose2d pose){ resetPose(pose); }, // Method to reset odometry (will be called if your auto has a starting pose)
[this](){ return getRobotRelativeSpeeds(); }, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
[this](frc::ChassisSpeeds speeds){ driveRobotRelative(speeds); }, // 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_mps, // Max module speed, in m/s
0.4_m, // Drive base radius in meters. Distance from robot center to furthest module.
ReplanningConfig() // Default path replanning config. See the API for the options here
),
this // Reference to this subsystem to set requirements
);
}
Ramsete
This example will use the default Ramsete configuration. If you would like to customize the b and zeta terms, there is another AutoBuilder configuration method with that option.
#include <pathplanner/lib/auto/AutoBuilder.h>
#include <pathplanner/lib/util/ReplanningConfig.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
using namespace pathplanner;
DriveSubsystem::DriveSubsystem(){
// Do all subsystem initialization here
// ...
// Configure the AutoBuilder last
AutoBuilder::configureRamsete(
[this](){ return getPose(); }, // Robot pose supplier
[this](frc::Pose2d pose){ resetPose(pose); }, // Method to reset odometry (will be called if your auto has a starting pose)
[this](){ return getRobotRelativeSpeeds(); }, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
[this](frc::ChassisSpeeds speeds){ driveRobotRelative(speeds); }, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
ReplanningConfig(), // Default path replanning config. See the API for the options here
this // Reference to this subsystem to set requirements
);
}
LTV
This example will use the default LTV configuration. If you would like to customize the qelems and relems terms, there is another AutoBuilder configuration method with that option.
#include <pathplanner/lib/auto/AutoBuilder.h>
#include <pathplanner/lib/util/ReplanningConfig.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
using namespace pathplanner;
DriveSubsystem::DriveSubsystem(){
// Do all subsystem initialization here
// ...
// Configure the AutoBuilder last
AutoBuilder::configureLTV(
[this](){ return getPose(); }, // Robot pose supplier
[this](frc::Pose2d pose){ resetPose(pose); }, // Method to reset odometry (will be called if your auto has a starting pose)
[this](){ return getRobotRelativeSpeeds(); }, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
[this](frc::ChassisSpeeds speeds){ driveRobotRelative(speeds); }, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
0.02_s, // Robot control loop period in seconds. Default is 0.02
ReplanningConfig(), // Default path replanning config. See the API for the options here
this // 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.
#include <pathplanner/lib/commands/PathPlannerAuto.h>
using namespace pathplanner;
frc2::CommandPtr RobotContainer::getAutonomousCommand(){
return PathPlannerAuto("Example Auto").ToPtr();
}