public class DriveSubsystem extends SubsystemBase {
public DriveSubsystem() {
// All other subsystem initialization
// ...
// Configure AutoBuilder last
AutoBuilder.configureHolonomic(
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
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
),
() -> {
// 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
);
}
}
#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>
#include <frc/DriverStation.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
),
[]() {
// 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
auto alliance = DriverStation::GetAlliance();
if (alliance) {
return alliance.value() == DriverStation::Alliance::kRed;
}
return false;
},
this // Reference to this subsystem to set requirements
);
}
from pathplannerlib.auto import AutoBuilder
from pathplannerlib.config import HolonomicPathFollowerConfig, ReplanningConfig, PIDConstants
from wpilib import DriverStation
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.shouldFlipPath, # Supplier to control path flipping based on alliance color
self # Reference to this subsystem to set requirements
)
def shouldFlipPath():
# 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
return DriverStation.getAlliance() == DriverStation.Alliance.kRed