Combining Motion Profiling and PID in Command-Based¶
Note
For a description of the WPILib PID control features used by these command-based wrappers, see PID Control in WPILib.
Note
Unlike the earlier version of PIDController
, the 2020 ProfiledPIDController
class runs synchronously, and is not handled in its own thread. Accordingly, changing its period
parameter will not change the actual frequency at which it runs in any of these wrapper classes. Users should never modify the period
parameter unless they are certain of what they are doing.
A common FRC controls solution is to pair a trapezoidal motion profile for setpoint generation with a PID controller for setpoint tracking. To facilitate this, WPILib includes its own ProfiledPIDController class. To further aid teams in integrating this functionality into their robots, the command-based framework contains two convenience wrappers for the ProfiledPIDController
class: ProfiledPIDSubsystem
, which integrates the controller into a subsystem, and ProfiledPIDCommand
, which integrates the controller into a command.
ProfiledPIDSubsystem¶
Note
In C++, the ProfiledPIDSubsystem
class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see The C++ Units Library.
The ProfiledPIDSubsystem
class (Java, C++) allows users to conveniently create a subsystem with a built-in PIDController. In order to use the ProfiledPIDSubsystem
class, users must create a subclass of it.
Creating a ProfiledPIDSubsystem¶
When subclassing ProfiledPIDSubsystem
, users must override two abstract methods to provide functionality that the class will use in its ordinary operation:
getMeasurement()¶
protected abstract double getMeasurement();
virtual double GetMeasurement() = 0;
The getMeasurement
method returns the current measurement of the process variable. The PIDSubsystem
will automatically call this method from its periodic()
block, and pass its value to the control loop.
Users should override this method to return whatever sensor reading they wish to use as their process variable measurement.
useOutput()¶
protected abstract void useOutput(double output, TrapezoidProfile.State setpoint);
virtual void UseOutput(double output, frc::TrapezoidProfile<Distance>::State setpoint) = 0;
The useOutput()
method consumes the output of the PID controller, and the current setpoint state (which is often useful for computing a feedforward). The PIDSubsystem
will automatically call this method from its periodic()
block, and pass it the computed output of the control loop.
Users should override this method to pass the final computed control output to their subsystem’s motors.
Passing In the Controller¶
Users must also pass in a ProfiledPIDController
to the ProfiledPIDSubsystem
base class through the superclass constructor call of their subclass. This serves to specify the PID gains, the motion profile constraints, and the period (if the user is using a non-standard main robot loop period).
Additional modifications (e.g. enabling continuous input) can be made to the controller in the constructor body by calling getController()
.
Using a ProfiledPIDSubsystem¶
Once an instance of a PIDSubsystem
subclass has been created, it can be used by commands through the following methods:
setGoal()¶
Note
If you wish to set the goal to a simple distance with an implicit target velocity of zero, an overload of setGoal()
exists that takes a single distance value, rather than a full motion profile state.
The setGoal()
method can be used to set the setpoint of the PIDSubsystem
. The subsystem will automatically track to the setpoint using the defined output:
// The subsystem will track to a goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsystem.setGoal(5, 3);
// The subsystem will track to a goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsyste.SetGoal({5_m, 3_mps});
enable() and disable()¶
The enable()
and disable()
methods enable and disable the automatic control of the ProfiledPIDSubsystem
. When the subsystem is enabled, it will automatically run the motion profile and the control loop and track to the goal. When it is disabled, no control is performed.
Additionally, the enable()
method resets the internal ProfiledPIDController
, and the disable()
method calls the user-defined useOutput() method with both output and setpoint set to 0
.
Full ProfiledPIDSubsystem Example¶
What does a PIDSubsystem look like when used in practice? The following examples are taken from the ArmBot example project (Java, C++):
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 | package edu.wpi.first.wpilibj.examples.armbot.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.ArmFeedforward;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
/**
* A robot arm subsystem that moves with a motion profile.
*/
public class ArmSubsystem extends ProfiledPIDSubsystem {
private final PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort);
private final Encoder m_encoder =
new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
private final ArmFeedforward m_feedforward =
new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
/**
* Create a new ArmSubsystem.
*/
public ArmSubsystem() {
super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints(
ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0);
m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
// Start arm at rest in neutral position
setGoal(ArmConstants.kArmOffsetRads);
}
@Override
public void useOutput(double output, TrapezoidProfile.State setpoint) {
// Calculate the feedforward from the sepoint
double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
// Add the feedforward to the PID output to get the motor output
m_motor.setVoltage(output + feedforward);
}
@Override
public double getMeasurement() {
return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
}
}
|
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 | #pragma once
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/controller/ArmFeedforward.h>
#include <frc2/command/ProfiledPIDSubsystem.h>
#include <units/units.h>
/**
* A robot arm subsystem that moves with a motion profile.
*/
class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
using State = frc::TrapezoidProfile<units::radians>::State;
public:
ArmSubsystem();
void UseOutput(double output, State setpoint) override;
units::radian_t GetMeasurement() override;
private:
frc::PWMVictorSPX m_motor;
frc::Encoder m_encoder;
frc::ArmFeedforward m_feedforward;
};
|
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 | #include "subsystems/ArmSubsystem.h"
#include "Constants.h"
using namespace ArmConstants;
using State = frc::TrapezoidProfile<units::radians>::State;
ArmSubsystem::ArmSubsystem()
: frc2::ProfiledPIDSubsystem<units::radians>(
frc::ProfiledPIDController<units::radians>(
kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
m_motor(kMotorPort),
m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
m_feedforward(kS, kCos, kV, kA) {
m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
// Start arm in neutral position
SetGoal(State{kArmOffset, 0_rad_per_s});
}
void ArmSubsystem::UseOutput(double output, State setpoint) {
// Calculate the feedforward from the sepoint
units::volt_t feedforward =
m_feedforward.Calculate(setpoint.position, setpoint.velocity);
// Add the feedforward to the PID output to get the motor output
m_motor.SetVoltage(units::volt_t(output) + feedforward);
}
units::radian_t ArmSubsystem::GetMeasurement() {
return units::radian_t(m_encoder.GetDistance()) + kArmOffset;
}
|
Using a ProfiledPIDSubsystem
with commands can be very simple:
63 64 65 66 67 68 69 | // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
// Move the arm to neutral position when the 'B' button is pressed.
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
|
33 34 35 36 37 38 39 40 | // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
frc2::JoystickButton(&m_driverController, 1)
.WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
// Move the arm to neutral position when the 'B' button is pressed.
frc2::JoystickButton(&m_driverController, 1)
.WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
{&m_arm});
|
ProfiledPIDCommand¶
Note
In C++, the ProfiledPIDCommand
class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see The C++ Units Library.
The ProfiledPIDCommand
class (Java, C++) allows users to easily create commands with a built-in ProfiledPIDController. As with ProfiledPIDSubsystem
, users can create a ProfiledPIDCommmand
by subclassing the ProfiledPIDCommand
class. However, as with many of the other command classes in the command-based library, users may want to save code by defining it inline.
Creating a PIDCommand¶
A ProfiledPIDCommand
can be created two ways - by subclassing the ProfiledPIDCommand
class, or by defining the command inline. Both methods ultimately extremely similar, and ultimately the choice of which to use comes down to where the user desires that the relevant code be located.
In either case, a ProfiledPIDCommand
is created by passing the necessary parameters to its constructor (if defining a subclass, this can be done with a super() call):
32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 | /**
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
* Goal velocity is specified.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param goalSource the controller's goal
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
Subsystem... requirements) {
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
m_controller = controller;
m_useOutput = useOutput;
m_measurement = measurementSource;
m_goal = goalSource;
m_requirements.addAll(Set.of(requirements));
}
|
39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 | public:
/**
* Creates a new PIDCommand, which controls the given output with a
* ProfiledPIDController.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param goalSource the controller's goal
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
std::function<Distance_t()> measurementSource,
std::function<State()> goalSource,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: m_controller{controller},
m_measurement{std::move(measurementSource)},
m_goal{std::move(goalSource)},
m_useOutput{std::move(useOutput)} {
this->AddRequirements(requirements);
|
controller¶
The controller
parameter is the ProfiledPIDController
object that will be used by the command. By passing this in, users can specify the PID gains, the motion profile constraints, and the period for the controller (if the user is using a nonstandard main robot loop period).
When subclassing ProfiledPIDCommand
, additional modifications (e.g. enabling continuous input) can be made to the controller in the constructor body by calling getController()
.
measurementSource¶
The measurementSource
parameter is a function (usually passed as a lambda) that returns the measurement of the process variable. Pasing in the measurementSource
function in ProfiledPIDCommand
is functionally analogous to overriding the getMeasurement() function in ProfiledPIDSubsystem
.
When subclassing ProfiledPIDCommand
, advanced users may further modify the measurement supplier by modifying the class’s m_measurement
field.
goalSource¶
The goalSource
parameter is a function (usually passed as a lambda) that returns the current goal state for the mechanism. If only a constant goal is needed, an overload exists that takes a constant goal rather than a supplier. Additionally, if goal velocities are desired to be zero, overloads exist that take a constant distance rather than a full profile state.
When subclassing ProfiledPIDCommand
, advanced users may further modify the setpoint supplier by modifying the class’s m_goal
field.
useOutput¶
The useOutput
parameter is a function (usually passed as a lambda) that consumes the output and setpoint state of the control loop. Passing in the useOutput
function in ProfiledPIDCommand
is functionally analogous to overriding the useOutput() function in ProfiledPIDSubsystem
.
When subclassing ProfiledPIDCommand
, advanced users may further modify the output consumer by modifying the class’s m_useOutput
field.
requirements¶
Like all inlineable commands, ProfiledPIDCommand
allows the user to specify its subsystem requirements as a constructor parameter.
Full ProfiledPIDCommand Example¶
What does a ProfiledPIDCommand
look like when used in practice? The following examples are from the GyroDriveCommands example project (Java, C++):
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 | package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
/**
* A command that will turn the robot to the specified angle using a motion profile.
*/
public class TurnToAngleProfiled extends ProfiledPIDCommand {
/**
* Turns to robot to the specified angle using a motion profile.
*
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
super(
new ProfiledPIDController(DriveConstants.kTurnP, DriveConstants.kTurnI,
DriveConstants.kTurnD, new TrapezoidProfile.Constraints(
DriveConstants.kMaxTurnRateDegPerS,
DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
(output, setpoint) -> drive.arcadeDrive(0, output),
// Require the drive
drive);
// Set the controller to be continuous (because it is an angle controller)
getController().enableContinuousInput(-180, 180);
// Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
// setpoint before it is considered as having reached the reference
getController()
.setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
}
@Override
public boolean isFinished() {
// End when the controller is at the reference.
return getController().atGoal();
}
}
|
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 | #pragma once
#include <frc2/command/CommandHelper.h>
#include <frc2/command/ProfiledPIDCommand.h>
#include "subsystems/DriveSubsystem.h"
/**
* A command that will turn the robot to the specified angle using a motion
* profile.
*/
class TurnToAngleProfiled
: public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
TurnToAngleProfiled> {
public:
/**
* Turns to robot to the specified angle using a motion profile.
*
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
TurnToAngleProfiled(units::degree_t targetAngleDegrees,
DriveSubsystem* drive);
bool IsFinished() override;
};
|
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 | #include "commands/TurnToAngleProfiled.h"
#include <frc/controller/ProfiledPIDController.h>
using namespace DriveConstants;
TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
DriveSubsystem* drive)
: CommandHelper(
frc::ProfiledPIDController<units::radians>(
kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}),
// Close loop on heading
[drive] { return drive->GetHeading(); },
// Set reference to target
target,
// Pipe output to turn robot
[drive](double output, auto setpointState) {
drive->ArcadeDrive(0, output);
},
// Require the drive
{drive}) {
// Set the controller to be continuous (because it is an angle controller)
GetController().EnableContinuousInput(-180_deg, 180_deg);
// Set the controller tolerance - the delta tolerance ensures the robot is
// stationary at the setpoint before it is considered as having reached the
// reference
GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
AddRequirements({drive});
}
bool TurnToAngleProfiled::IsFinished() { return GetController().AtGoal(); }
|