WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
frc::HolonomicDriveController Class Reference

This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain (i.e. More...

#include <frc/controller/HolonomicDriveController.h>

Public Member Functions

constexpr HolonomicDriveController (PIDController xController, PIDController yController, ProfiledPIDController< units::radian > thetaController)
 Constructs a holonomic drive controller.
 
constexpr HolonomicDriveController (const HolonomicDriveController &)=default
 
constexpr HolonomicDriveControlleroperator= (const HolonomicDriveController &)=default
 
constexpr HolonomicDriveController (HolonomicDriveController &&)=default
 
constexpr HolonomicDriveControlleroperator= (HolonomicDriveController &&)=default
 
constexpr bool AtReference () const
 Returns true if the pose error is within tolerance of the reference.
 
constexpr void SetTolerance (const Pose2d &tolerance)
 Sets the pose error which is considered tolerable for use with AtReference().
 
constexpr ChassisSpeeds Calculate (const Pose2d &currentPose, const Pose2d &trajectoryPose, units::meters_per_second_t desiredLinearVelocity, const Rotation2d &desiredHeading)
 Returns the next output of the holonomic drive controller.
 
constexpr ChassisSpeeds Calculate (const Pose2d &currentPose, const Trajectory::State &desiredState, const Rotation2d &desiredHeading)
 Returns the next output of the holonomic drive controller.
 
constexpr void SetEnabled (bool enabled)
 Enables and disables the controller for troubleshooting purposes.
 
constexpr PIDControllergetXController ()
 Returns the X PIDController.
 
constexpr PIDControllergetYController ()
 Returns the Y PIDController.
 
constexpr ProfiledPIDController< units::radian > & getThetaController ()
 Returns the rotation ProfiledPIDController.
 
constexpr PIDControllerGetXController ()
 Returns the X PIDController.
 
constexpr PIDControllerGetYController ()
 Returns the Y PIDController.
 
constexpr ProfiledPIDController< units::radian > & GetThetaController ()
 Returns the rotation ProfiledPIDController.
 

Detailed Description

This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain (i.e.

swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve compared to skid-steer style drivetrains because it is possible to individually control forward, sideways, and angular velocity.

The holonomic drive controller takes in one PID controller for each direction, forward and sideways, and one profiled PID controller for the angular direction. Because the heading dynamics are decoupled from translations, users can specify a custom heading that the drivetrain should point toward. This heading reference is profiled for smoothness.

Constructor & Destructor Documentation

◆ HolonomicDriveController() [1/3]

frc::HolonomicDriveController::HolonomicDriveController ( PIDController xController,
PIDController yController,
ProfiledPIDController< units::radian > thetaController )
inlineconstexpr

Constructs a holonomic drive controller.

Parameters
xControllerA PID Controller to respond to error in the field-relative x direction.
yControllerA PID Controller to respond to error in the field-relative y direction.
thetaControllerA profiled PID controller to respond to error in angle.

◆ HolonomicDriveController() [2/3]

frc::HolonomicDriveController::HolonomicDriveController ( const HolonomicDriveController & )
constexprdefault

◆ HolonomicDriveController() [3/3]

frc::HolonomicDriveController::HolonomicDriveController ( HolonomicDriveController && )
constexprdefault

Member Function Documentation

◆ AtReference()

bool frc::HolonomicDriveController::AtReference ( ) const
inlineconstexpr

Returns true if the pose error is within tolerance of the reference.

◆ Calculate() [1/2]

ChassisSpeeds frc::HolonomicDriveController::Calculate ( const Pose2d & currentPose,
const Pose2d & trajectoryPose,
units::meters_per_second_t desiredLinearVelocity,
const Rotation2d & desiredHeading )
inlineconstexpr

Returns the next output of the holonomic drive controller.

Parameters
currentPoseThe current pose, as measured by odometry or pose estimator.
trajectoryPoseThe desired trajectory pose, as sampled for the current timestep.
desiredLinearVelocityThe desired linear velocity.
desiredHeadingThe desired heading.
Returns
The next output of the holonomic drive controller.

◆ Calculate() [2/2]

ChassisSpeeds frc::HolonomicDriveController::Calculate ( const Pose2d & currentPose,
const Trajectory::State & desiredState,
const Rotation2d & desiredHeading )
inlineconstexpr

Returns the next output of the holonomic drive controller.

Parameters
currentPoseThe current pose, as measured by odometry or pose estimator.
desiredStateThe desired trajectory pose, as sampled for the current timestep.
desiredHeadingThe desired heading.
Returns
The next output of the holonomic drive controller.

◆ GetThetaController()

ProfiledPIDController< units::radian > & frc::HolonomicDriveController::GetThetaController ( )
inlineconstexpr

Returns the rotation ProfiledPIDController.

◆ getThetaController()

ProfiledPIDController< units::radian > & frc::HolonomicDriveController::getThetaController ( )
inlineconstexpr

Returns the rotation ProfiledPIDController.

Deprecated
Use GetThetaController() instead.

◆ GetXController()

PIDController & frc::HolonomicDriveController::GetXController ( )
inlineconstexpr

Returns the X PIDController.

◆ getXController()

PIDController & frc::HolonomicDriveController::getXController ( )
inlineconstexpr

Returns the X PIDController.

Deprecated
Use GetXController() instead.

◆ GetYController()

PIDController & frc::HolonomicDriveController::GetYController ( )
inlineconstexpr

Returns the Y PIDController.

◆ getYController()

PIDController & frc::HolonomicDriveController::getYController ( )
inlineconstexpr

Returns the Y PIDController.

Deprecated
Use GetYController() instead.

◆ operator=() [1/2]

HolonomicDriveController & frc::HolonomicDriveController::operator= ( const HolonomicDriveController & )
constexprdefault

◆ operator=() [2/2]

HolonomicDriveController & frc::HolonomicDriveController::operator= ( HolonomicDriveController && )
constexprdefault

◆ SetEnabled()

void frc::HolonomicDriveController::SetEnabled ( bool enabled)
inlineconstexpr

Enables and disables the controller for troubleshooting purposes.

When Calculate() is called on a disabled controller, only feedforward values are returned.

Parameters
enabledIf the controller is enabled or not.

◆ SetTolerance()

void frc::HolonomicDriveController::SetTolerance ( const Pose2d & tolerance)
inlineconstexpr

Sets the pose error which is considered tolerable for use with AtReference().

Parameters
tolerancePose error which is tolerable.

The documentation for this class was generated from the following file: