WPILibC++ 2023.4.3
|
A command that uses two PID controllers (PIDController) and a profiled PID controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a mecanum drive. More...
#include <frc2/command/MecanumControllerCommand.h>
Public Member Functions | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< frc::Rotation2d()> desiredRotation, units::meters_per_second_t maxWheelVelocity, std::function< frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds, frc2::PIDController frontLeftController, frc2::PIDController rearLeftController, frc2::PIDController frontRightController, frc2::PIDController rearRightController, std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> output, std::initializer_list< Subsystem * > requirements) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, units::meters_per_second_t maxWheelVelocity, std::function< frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds, frc2::PIDController frontLeftController, frc2::PIDController rearLeftController, frc2::PIDController frontRightController, frc2::PIDController rearRightController, std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> output, std::initializer_list< Subsystem * > requirements) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< frc::Rotation2d()> desiredRotation, units::meters_per_second_t maxWheelVelocity, std::function< frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds, frc2::PIDController frontLeftController, frc2::PIDController rearLeftController, frc2::PIDController frontRightController, frc2::PIDController rearRightController, std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> output, std::span< Subsystem *const > requirements={}) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, units::meters_per_second_t maxWheelVelocity, std::function< frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds, frc2::PIDController frontLeftController, frc2::PIDController rearLeftController, frc2::PIDController frontRightController, frc2::PIDController rearRightController, std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> output, std::span< Subsystem *const > requirements={}) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< frc::Rotation2d()> desiredRotation, units::meters_per_second_t maxWheelVelocity, std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> output, std::initializer_list< Subsystem * > requirements) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, units::meters_per_second_t maxWheelVelocity, std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> output, std::initializer_list< Subsystem * > requirements) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< frc::Rotation2d()> desiredRotation, units::meters_per_second_t maxWheelVelocity, std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> output, std::span< Subsystem *const > requirements={}) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
MecanumControllerCommand (frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, units::meters_per_second_t maxWheelVelocity, std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> output, std::span< Subsystem *const > requirements={}) | |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. More... | |
void | Initialize () override |
The initial subroutine of a command. More... | |
void | Execute () override |
The main body of a command. More... | |
void | End (bool interrupted) override |
The action to take when the command ends. More... | |
bool | IsFinished () override |
Whether the command has finished. More... | |
Public Member Functions inherited from frc2::CommandHelper< CommandBase, MecanumControllerCommand > | |
CommandHelper ()=default | |
CommandPtr | ToPtr () &&override |
Public Member Functions inherited from frc2::CommandBase | |
void | AddRequirements (std::initializer_list< Subsystem * > requirements) |
Adds the specified Subsystem requirements to the command. More... | |
void | AddRequirements (std::span< Subsystem *const > requirements) |
Adds the specified Subsystem requirements to the command. More... | |
void | AddRequirements (wpi::SmallSet< Subsystem *, 4 > requirements) |
Adds the specified Subsystem requirements to the command. More... | |
void | AddRequirements (Subsystem *requirement) |
Adds the specified Subsystem requirement to the command. More... | |
wpi::SmallSet< Subsystem *, 4 > | GetRequirements () const override |
Gets the Subsystem requirements of the command. More... | |
void | SetName (std::string_view name) override |
Sets the name of this Command. More... | |
std::string | GetName () const override |
Gets the name of this Command. More... | |
std::string | GetSubsystem () const |
Gets the subsystem name of this Command. More... | |
void | SetSubsystem (std::string_view subsystem) |
Sets the subsystem name of this Command. More... | |
void | InitSendable (wpi::SendableBuilder &builder) override |
Initializes this Sendable object. More... | |
Public Member Functions inherited from frc2::Command | |
Command ()=default | |
virtual | ~Command () |
Command (const Command &)=default | |
Command & | operator= (const Command &rhs) |
Command (Command &&)=default | |
Command & | operator= (Command &&)=default |
virtual void | Initialize () |
The initial subroutine of a command. More... | |
virtual void | Execute () |
The main body of a command. More... | |
virtual void | End (bool interrupted) |
The action to take when the command ends. More... | |
virtual bool | IsFinished () |
Whether the command has finished. More... | |
virtual wpi::SmallSet< Subsystem *, 4 > | GetRequirements () const =0 |
Specifies the set of subsystems used by this command. More... | |
CommandPtr | WithTimeout (units::second_t duration) && |
Decorates this command with a timeout. More... | |
CommandPtr | Until (std::function< bool()> condition) && |
Decorates this command with an interrupt condition. More... | |
CommandPtr | WithInterrupt (std::function< bool()> condition) && |
Decorates this command with an interrupt condition. More... | |
CommandPtr | BeforeStarting (std::function< void()> toRun, std::initializer_list< Subsystem * > requirements) && |
Decorates this command with a runnable to run before this command starts. More... | |
CommandPtr | BeforeStarting (std::function< void()> toRun, std::span< Subsystem *const > requirements={}) && |
Decorates this command with a runnable to run before this command starts. More... | |
CommandPtr | AndThen (std::function< void()> toRun, std::initializer_list< Subsystem * > requirements) && |
Decorates this command with a runnable to run after the command finishes. More... | |
CommandPtr | AndThen (std::function< void()> toRun, std::span< Subsystem *const > requirements={}) && |
Decorates this command with a runnable to run after the command finishes. More... | |
PerpetualCommand | Perpetually () && |
CommandPtr | Repeatedly () && |
Decorates this command to run repeatedly, restarting it when it ends, until this command is interrupted. More... | |
CommandPtr | AsProxy () && |
Decorates this command to run "by proxy" by wrapping it in a ProxyCommand. More... | |
CommandPtr | Unless (std::function< bool()> condition) && |
Decorates this command to only run if this condition is not met. More... | |
CommandPtr | IgnoringDisable (bool doesRunWhenDisabled) && |
Decorates this command to run or stop when disabled. More... | |
CommandPtr | WithInterruptBehavior (Command::InterruptionBehavior interruptBehavior) && |
Decorates this command to run or stop when disabled. More... | |
CommandPtr | FinallyDo (std::function< void(bool)> end) && |
Decorates this command with a lambda to call on interrupt or end, following the command's inherent Command::End(bool) method. More... | |
CommandPtr | HandleInterrupt (std::function< void()> handler) && |
Decorates this command with a lambda to call on interrupt, following the command's inherent Command::End(bool) method. More... | |
CommandPtr | WithName (std::string_view name) && |
Decorates this Command with a name. More... | |
void | Schedule () |
Schedules this command. More... | |
void | Cancel () |
Cancels this command. More... | |
bool | IsScheduled () const |
Whether or not the command is currently scheduled. More... | |
bool | HasRequirement (Subsystem *requirement) const |
Whether the command requires a given subsystem. More... | |
bool | IsComposed () const |
Whether the command is currently grouped in a command group. More... | |
void | SetComposed (bool isComposed) |
Sets whether the command is currently composed in a command composition. More... | |
bool | IsGrouped () const |
Whether the command is currently grouped in a command group. More... | |
void | SetGrouped (bool grouped) |
Sets whether the command is currently grouped in a command group. More... | |
virtual bool | RunsWhenDisabled () const |
Whether the given command should run when the robot is disabled. More... | |
virtual InterruptionBehavior | GetInterruptionBehavior () const |
How the command behaves when another command with a shared requirement is scheduled. More... | |
virtual std::string | GetName () const |
Gets the name of this Command. More... | |
virtual void | SetName (std::string_view name) |
Sets the name of this Command. More... | |
virtual CommandPtr | ToPtr () &&=0 |
Transfers ownership of this command to a unique pointer. More... | |
Public Member Functions inherited from wpi::Sendable | |
virtual | ~Sendable ()=default |
virtual void | InitSendable (SendableBuilder &builder)=0 |
Initializes this Sendable object. More... | |
Public Member Functions inherited from wpi::SendableHelper< CommandBase > | |
SendableHelper (const SendableHelper &rhs)=default | |
SendableHelper (SendableHelper &&rhs) | |
SendableHelper & | operator= (const SendableHelper &rhs)=default |
SendableHelper & | operator= (SendableHelper &&rhs) |
Additional Inherited Members | |
Public Types inherited from frc2::Command | |
enum class | InterruptionBehavior { kCancelSelf , kCancelIncoming } |
An enum describing the command's behavior when another command with a shared requirement is scheduled. More... | |
Protected Member Functions inherited from frc2::CommandHelper< CommandBase, MecanumControllerCommand > | |
std::unique_ptr< Command > | TransferOwnership () &&override |
Protected Member Functions inherited from frc2::CommandBase | |
CommandBase () | |
virtual std::unique_ptr< Command > | TransferOwnership () &&=0 |
Transfers ownership of this command to a unique pointer. More... | |
Protected Member Functions inherited from wpi::SendableHelper< CommandBase > | |
SendableHelper ()=default | |
~SendableHelper () | |
Protected Attributes inherited from frc2::CommandBase | |
wpi::SmallSet< Subsystem *, 4 > | m_requirements |
Protected Attributes inherited from frc2::Command | |
bool | m_isComposed = false |
A command that uses two PID controllers (PIDController) and a profiled PID controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a mecanum drive.
The command handles trajectory-following, Velocity PID calculations, and feedforwards internally. This is intended to be a more-or-less "complete solution" that can be used by teams without a great deal of controls expertise.
Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID functionality of a "smart" motor controller) may use the secondary constructor that omits the PID and feedforward functionality, returning only the raw wheel speeds from the PID controllers.
The robot angle controller does not follow the angle given by the trajectory but rather goes to the angle given in the final state of the trajectory.
This class is provided by the NewCommands VendorDep
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::SimpleMotorFeedforward< units::meters > | feedforward, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
std::function< frc::Rotation2d()> | desiredRotation, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< frc::MecanumDriveWheelSpeeds()> | currentWheelSpeeds, | ||
frc2::PIDController | frontLeftController, | ||
frc2::PIDController | rearLeftController, | ||
frc2::PIDController | frontRightController, | ||
frc2::PIDController | rearRightController, | ||
std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> | output, | ||
std::initializer_list< Subsystem * > | requirements | ||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
PID control and feedforward are handled internally. Outputs are scaled from -12 to 12 as a voltage output to the motor.
Note: The controllers will not set the outputVolts to zero upon completion of the path this is left to the user, since it is not appropriate for paths with nonstationary endstates.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose, provided by the odometry class. |
feedforward | The feedforward to use for the drivetrain. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
desiredRotation | The angle that the robot should be facing. This is sampled at each time step. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
frontLeftController | The front left wheel velocity PID. |
rearLeftController | The rear left wheel velocity PID. |
frontRightController | The front right wheel velocity PID. |
rearRightController | The rear right wheel velocity PID. |
currentWheelSpeeds | A MecanumDriveWheelSpeeds object containing the current wheel speeds. |
output | The output of the velocity PIDs. |
requirements | The subsystems to require. |
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::SimpleMotorFeedforward< units::meters > | feedforward, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< frc::MecanumDriveWheelSpeeds()> | currentWheelSpeeds, | ||
frc2::PIDController | frontLeftController, | ||
frc2::PIDController | rearLeftController, | ||
frc2::PIDController | frontRightController, | ||
frc2::PIDController | rearRightController, | ||
std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> | output, | ||
std::initializer_list< Subsystem * > | requirements | ||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
PID control and feedforward are handled internally. Outputs are scaled from -12 to 12 as a voltage output to the motor.
Note: The controllers will not set the outputVolts to zero upon completion of the path this is left to the user, since it is not appropriate for paths with nonstationary endstates.
Note 2: The final rotation of the robot will be set to the rotation of the final pose in the trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior is desired, the other constructor with a supplier for rotation should be used.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose, provided by the odometry class. |
feedforward | The feedforward to use for the drivetrain. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
frontLeftController | The front left wheel velocity PID. |
rearLeftController | The rear left wheel velocity PID. |
frontRightController | The front right wheel velocity PID. |
rearRightController | The rear right wheel velocity PID. |
currentWheelSpeeds | A MecanumDriveWheelSpeeds object containing the current wheel speeds. |
output | The output of the velocity PIDs. |
requirements | The subsystems to require. |
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::SimpleMotorFeedforward< units::meters > | feedforward, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
std::function< frc::Rotation2d()> | desiredRotation, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< frc::MecanumDriveWheelSpeeds()> | currentWheelSpeeds, | ||
frc2::PIDController | frontLeftController, | ||
frc2::PIDController | rearLeftController, | ||
frc2::PIDController | frontRightController, | ||
frc2::PIDController | rearRightController, | ||
std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> | output, | ||
std::span< Subsystem *const > | requirements = {} |
||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
PID control and feedforward are handled internally. Outputs are scaled from -12 to 12 as a voltage output to the motor.
Note: The controllers will not set the outputVolts to zero upon completion of the path this is left to the user, since it is not appropriate for paths with nonstationary endstates.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose, provided by the odometry class. |
feedforward | The feedforward to use for the drivetrain. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
desiredRotation | The angle that the robot should be facing. This is sampled at each time step. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
frontLeftController | The front left wheel velocity PID. |
rearLeftController | The rear left wheel velocity PID. |
frontRightController | The front right wheel velocity PID. |
rearRightController | The rear right wheel velocity PID. |
currentWheelSpeeds | A MecanumDriveWheelSpeeds object containing the current wheel speeds. |
output | The output of the velocity PIDs. |
requirements | The subsystems to require. |
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::SimpleMotorFeedforward< units::meters > | feedforward, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< frc::MecanumDriveWheelSpeeds()> | currentWheelSpeeds, | ||
frc2::PIDController | frontLeftController, | ||
frc2::PIDController | rearLeftController, | ||
frc2::PIDController | frontRightController, | ||
frc2::PIDController | rearRightController, | ||
std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> | output, | ||
std::span< Subsystem *const > | requirements = {} |
||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
PID control and feedforward are handled internally. Outputs are scaled from -12 to 12 as a voltage output to the motor.
Note: The controllers will not set the outputVolts to zero upon completion of the path this is left to the user, since it is not appropriate for paths with nonstationary endstates.
Note 2: The final rotation of the robot will be set to the rotation of the final pose in the trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior is desired, the other constructor with a supplier for rotation should be used.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose, provided by the odometry class. |
feedforward | The feedforward to use for the drivetrain. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
frontLeftController | The front left wheel velocity PID. |
rearLeftController | The rear left wheel velocity PID. |
frontRightController | The front right wheel velocity PID. |
rearRightController | The rear right wheel velocity PID. |
currentWheelSpeeds | A MecanumDriveWheelSpeeds object containing the current wheel speeds. |
output | The output of the velocity PIDs. |
requirements | The subsystems to require. |
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
std::function< frc::Rotation2d()> | desiredRotation, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> | output, | ||
std::initializer_list< Subsystem * > | requirements | ||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
The user should implement a velocity PID on the desired output wheel velocities.
Note: The controllers will not set the outputVolts to zero upon completion of the path - this is left to the user, since it is not appropriate for paths with nonstationary end-states.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose - use one of the odometry classes to provide this. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
desiredRotation | The angle that the robot should be facing. This is sampled at each time step. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
output | The output of the position PIDs. |
requirements | The subsystems to require. |
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> | output, | ||
std::initializer_list< Subsystem * > | requirements | ||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
The user should implement a velocity PID on the desired output wheel velocities.
Note: The controllers will not set the outputVolts to zero upon completion of the path - this is left to the user, since it is not appropriate for paths with nonstationary end-states.
Note 2: The final rotation of the robot will be set to the rotation of the final pose in the trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior is desired, the other constructor with a supplier for rotation should be used.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose - use one of the odometry classes to provide this. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
output | The output of the position PIDs. |
requirements | The subsystems to require. |
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
std::function< frc::Rotation2d()> | desiredRotation, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> | output, | ||
std::span< Subsystem *const > | requirements = {} |
||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
The user should implement a velocity PID on the desired output wheel velocities.
Note: The controllers will not set the outputVolts to zero upon completion of the path - this is left to the user, since it is not appropriate for paths with nonstationary end-states.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose - use one of the odometry classes to provide this. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
desiredRotation | The angle that the robot should be facing. This is sampled at every time step. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
output | The output of the position PIDs. |
requirements | The subsystems to require. |
frc2::MecanumControllerCommand::MecanumControllerCommand | ( | frc::Trajectory | trajectory, |
std::function< frc::Pose2d()> | pose, | ||
frc::MecanumDriveKinematics | kinematics, | ||
frc2::PIDController | xController, | ||
frc2::PIDController | yController, | ||
frc::ProfiledPIDController< units::radians > | thetaController, | ||
units::meters_per_second_t | maxWheelVelocity, | ||
std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> | output, | ||
std::span< Subsystem *const > | requirements = {} |
||
) |
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
The user should implement a velocity PID on the desired output wheel velocities.
Note: The controllers will not set the outputVolts to zero upon completion of the path - this is left to the user, since it is not appropriate for paths with nonstationary end-states.
Note2: The final rotation of the robot will be set to the rotation of the final pose in the trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior is desired, the other constructor with a supplier for rotation should be used.
trajectory | The trajectory to follow. |
pose | A function that supplies the robot pose - use one of the odometry classes to provide this. |
kinematics | The kinematics for the robot drivetrain. |
xController | The Trajectory Tracker PID controller for the robot's x position. |
yController | The Trajectory Tracker PID controller for the robot's y position. |
thetaController | The Trajectory Tracker PID controller for angle for the robot. |
maxWheelVelocity | The maximum velocity of a drivetrain wheel. |
output | The output of the position PIDs. |
requirements | The subsystems to require. |
|
overridevirtual |
The action to take when the command ends.
Called when either the command finishes normally, or when it interrupted/canceled.
interrupted | whether the command was interrupted/canceled |
Reimplemented from frc2::Command.
|
overridevirtual |
The main body of a command.
Called repeatedly while the command is scheduled.
Reimplemented from frc2::Command.
|
overridevirtual |
The initial subroutine of a command.
Called once when the command is initially scheduled.
Reimplemented from frc2::Command.
|
overridevirtual |
Whether the command has finished.
Once a command finishes, the scheduler will call its end() method and un-schedule it.
Reimplemented from frc2::Command.