WPILibC++ 2023.4.3-108-ge5452e3
frc::RamseteController Class Reference

Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model to a desired pose along a two-dimensional trajectory. More...

#include <frc/controller/RamseteController.h>

Public Types

using b_unit = units::compound_unit< units::squared< units::radians >, units::inverse< units::squared< units::meters > > >
 
using zeta_unit = units::inverse< units::radians >
 

Public Member Functions

 RamseteController (units::unit_t< b_unit > b, units::unit_t< zeta_unit > zeta)
 Construct a Ramsete unicycle controller. More...
 
 RamseteController ()
 Construct a Ramsete unicycle controller. More...
 
bool AtReference () const
 Returns true if the pose error is within tolerance of the reference. More...
 
void SetTolerance (const Pose2d &poseTolerance)
 Sets the pose error which is considered tolerable for use with AtReference(). More...
 
ChassisSpeeds Calculate (const Pose2d &currentPose, const Pose2d &poseRef, units::meters_per_second_t linearVelocityRef, units::radians_per_second_t angularVelocityRef)
 Returns the next output of the Ramsete controller. More...
 
ChassisSpeeds Calculate (const Pose2d &currentPose, const Trajectory::State &desiredState)
 Returns the next output of the Ramsete controller. More...
 
void SetEnabled (bool enabled)
 Enables and disables the controller for troubleshooting purposes. More...
 

Detailed Description

Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model to a desired pose along a two-dimensional trajectory.

Why would we need a nonlinear control law in addition to the linear ones we have used so far like PID? If we use the original approach with PID controllers for left and right position and velocity states, the controllers only deal with the local pose. If the robot deviates from the path, there is no way for the controllers to correct and the robot may not reach the desired global pose. This is due to multiple endpoints existing for the robot which have the same encoder path arc lengths.

Instead of using wheel path arc lengths (which are in the robot's local coordinate frame), nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this extra information to guide a linear reference tracker like the PID controllers back in by adjusting the references of the PID controllers.

The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y, and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e Mobile per i SErvizi e le TEcnologie").

See https://file.tavsys.net/control/controls-engineering-in-frc.pdf section on Ramsete unicycle controller for a derivation and analysis.

Member Typedef Documentation

◆ b_unit

◆ zeta_unit

Constructor & Destructor Documentation

◆ RamseteController() [1/2]

frc::RamseteController::RamseteController ( units::unit_t< b_unit b,
units::unit_t< zeta_unit zeta 
)

Construct a Ramsete unicycle controller.

Parameters
bTuning parameter (b > 0 rad²/m²) for which larger values make convergence more aggressive like a proportional term.
zetaTuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger values provide more damping in response.

◆ RamseteController() [2/2]

frc::RamseteController::RamseteController ( )

Construct a Ramsete unicycle controller.

The default arguments for b and zeta of 2.0 rad²/m² and 0.7 rad⁻¹ have been well-tested to produce desirable results.

Member Function Documentation

◆ AtReference()

bool frc::RamseteController::AtReference ( ) const

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

◆ Calculate() [1/2]

ChassisSpeeds frc::RamseteController::Calculate ( const Pose2d currentPose,
const Pose2d poseRef,
units::meters_per_second_t  linearVelocityRef,
units::radians_per_second_t  angularVelocityRef 
)

Returns the next output of the Ramsete controller.

The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

Parameters
currentPoseThe current pose.
poseRefThe desired pose.
linearVelocityRefThe desired linear velocity.
angularVelocityRefThe desired angular velocity.

◆ Calculate() [2/2]

ChassisSpeeds frc::RamseteController::Calculate ( const Pose2d currentPose,
const Trajectory::State desiredState 
)

Returns the next output of the Ramsete controller.

The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

Parameters
currentPoseThe current pose.
desiredStateThe desired pose, linear velocity, and angular velocity from a trajectory.

◆ SetEnabled()

void frc::RamseteController::SetEnabled ( bool  enabled)

Enables and disables the controller for troubleshooting purposes.

Parameters
enabledIf the controller is enabled or not.

◆ SetTolerance()

void frc::RamseteController::SetTolerance ( const Pose2d poseTolerance)

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

Parameters
poseTolerancePose error which is tolerable.

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