Combining Motion Profiling and PID Control with ProfiledPIDController
הערה
For a guide on implementing the ProfiledPIDController
class in the command-based framework framework, see Combining Motion Profiling and PID in Command-Based.
In the previous article, we saw how to use the TrapezoidProfile
class to create and use a trapezoidal motion profile. The example code from that article demonstrates manually composing the TrapezoidProfile
class with the external PID control feature of a ”smart“ motor controller.
This combination of functionality (a motion profile for generating setpoints combined with a PID controller for following them) is extremely common. To facilitate this, WPILib comes with a ProfiledPIDController
class (Java, C++) that does most of the work of combining these two functionalities. The API of the ProfiledPIDController
is very similar to that of the PIDController
, allowing users to add motion profiling to a PID-controlled mechanism with very few changes to their code.
Using the ProfiledPIDController class
הערה
In C++, the ProfiledPIDController
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.
הערה
Much of the functionality of ProfiledPIDController
is effectively identical to that of PIDController
. Accordingly, this article will only cover features that are substantially-changed to accommodate the motion profiling functionality. For information on standard PIDController
features, see בקרת PID עם WPILib.
Constructing a ProfiledPIDController
הערה
C++ is often able to infer the type of the inner classes, and thus a simple initializer list (without the class name) can be sent as a parameter. The full class name is included in the example below for clarity.
Creating a ProfiledPIDController
is nearly identical to creating a PIDController. The only difference is the need to supply a set of trapezoidal profile constraints, which will be automatically forwarded to the internally-generated TrapezoidProfile
instances:
// Creates a ProfiledPIDController
// Max velocity is 5 meters per second
// Max acceleration is 10 meters per second
ProfiledPIDController controller = new ProfiledPIDController(
kP, kI, kD,
new TrapezoidProfile.Constraints(5, 10));
// Creates a ProfiledPIDController
// Max velocity is 5 meters per second
// Max acceleration is 10 meters per second
frc::ProfiledPIDController<units::meters> controller(
kP, kI, kD,
frc::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq});
Goal vs Setpoint
A major difference between a standard PIDController
and a ProfiledPIDController
is that the actual setpoint of the control loop is not directly specified by the user. Rather, the user specifies a goal position or state, and the setpoint for the controller is computed automatically from the generated motion profile between the current state and the goal. So, while the user-side call looks mostly identical:
// Calculates the output of the PID algorithm based on the sensor reading
// and sends it to a motor
motor.set(controller.calculate(encoder.getDistance(), goal));
// Calculates the output of the PID algorithm based on the sensor reading
// and sends it to a motor
motor.Set(controller.Calculate(encoder.GetDistance(), goal));
The specified goal
value (which can be either a position value or a TrapezoidProfile.State
, if nonzero velocity is desired) is not necessarily the current setpoint of the loop - rather, it is the eventual setpoint once the generated profile terminates.
Getting/Using the Setpoint
Since the ProfiledPIDController
goal differs from the setpoint, is if often desirable to poll the current setpoint of the controller (for instance, to get values to use with feedforward). This can be done with the getSetpoint()
method.
The returned setpoint might then be used as in the following example:
double lastSpeed = 0;
double lastTime = Timer.getFPGATimestamp();
// Controls a simple motor's position using a SimpleMotorFeedforward
// and a ProfiledPIDController
public void goToPosition(double goalPosition) {
double pidVal = controller.calculate(encoder.getDistance(), goalPosition);
double acceleration = (controller.getSetpoint().velocity - lastSpeed) / (Timer.getFPGATimestamp() - lastTime);
motor.setVoltage(
pidVal
+ feedforward.calculate(controller.getSetpoint().velocity, acceleration));
lastSpeed = controller.getSetpoint().velocity;
lastTime = Timer.getFPGATimestamp();
}
units::meters_per_second_t lastSpeed = 0_mps;
units::second_t lastTime = frc2::Timer::GetFPGATimestamp();
// Controls a simple motor's position using a SimpleMotorFeedforward
// and a ProfiledPIDController
void GoToPosition(units::meter_t goalPosition) {
auto pidVal = controller.Calculate(units::meter_t{encoder.GetDistance()}, goalPosition);
auto acceleration = (controller.GetSetpoint().velocity - lastSpeed) /
(frc2::Timer::GetFPGATimestamp() - lastTime);
motor.SetVoltage(
pidVal +
feedforward.Calculate(controller.GetSetpoint().velocity, acceleration));
lastSpeed = controller.GetSetpoint().velocity;
lastTime = frc2::Timer::GetFPGATimestamp();
}
Complete Usage Example
A more complete example of ProfiledPIDController
usage is provided in the ElevatorProfilePID example project (Java, C++):
5package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
6
7import edu.wpi.first.math.controller.ProfiledPIDController;
8import edu.wpi.first.math.trajectory.TrapezoidProfile;
9import edu.wpi.first.wpilibj.Encoder;
10import edu.wpi.first.wpilibj.Joystick;
11import edu.wpi.first.wpilibj.TimedRobot;
12import edu.wpi.first.wpilibj.motorcontrol.MotorController;
13import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
14
15public class Robot extends TimedRobot {
16 private static double kDt = 0.02;
17
18 private final Joystick m_joystick = new Joystick(1);
19 private final Encoder m_encoder = new Encoder(1, 2);
20 private final MotorController m_motor = new PWMSparkMax(1);
21
22 // Create a PID controller whose setpoint's change is subject to maximum
23 // velocity and acceleration constraints.
24 private final TrapezoidProfile.Constraints m_constraints =
25 new TrapezoidProfile.Constraints(1.75, 0.75);
26 private final ProfiledPIDController m_controller =
27 new ProfiledPIDController(1.3, 0.0, 0.7, m_constraints, kDt);
28
29 @Override
30 public void robotInit() {
31 m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
32 }
33
34 @Override
35 public void teleopPeriodic() {
36 if (m_joystick.getRawButtonPressed(2)) {
37 m_controller.setGoal(5);
38 } else if (m_joystick.getRawButtonPressed(3)) {
39 m_controller.setGoal(0);
40 }
41
42 // Run controller and update motor output
43 m_motor.set(m_controller.calculate(m_encoder.getDistance()));
44 }
45}
5#include <numbers>
6
7#include <frc/Encoder.h>
8#include <frc/Joystick.h>
9#include <frc/TimedRobot.h>
10#include <frc/controller/ProfiledPIDController.h>
11#include <frc/motorcontrol/PWMSparkMax.h>
12#include <frc/trajectory/TrapezoidProfile.h>
13#include <units/acceleration.h>
14#include <units/length.h>
15#include <units/time.h>
16#include <units/velocity.h>
17
18class Robot : public frc::TimedRobot {
19 public:
20 static constexpr units::second_t kDt = 20_ms;
21
22 Robot() {
23 m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
24 }
25
26 void TeleopPeriodic() override {
27 if (m_joystick.GetRawButtonPressed(2)) {
28 m_controller.SetGoal(5_m);
29 } else if (m_joystick.GetRawButtonPressed(3)) {
30 m_controller.SetGoal(0_m);
31 }
32
33 // Run controller and update motor output
34 m_motor.Set(
35 m_controller.Calculate(units::meter_t{m_encoder.GetDistance()}));
36 }
37
38 private:
39 frc::Joystick m_joystick{1};
40 frc::Encoder m_encoder{1, 2};
41 frc::PWMSparkMax m_motor{1};
42
43 // Create a PID controller whose setpoint's change is subject to maximum
44 // velocity and acceleration constraints.
45 frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
46 0.75_mps_sq};
47 frc::ProfiledPIDController<units::meters> m_controller{1.3, 0.0, 0.7,
48 m_constraints, kDt};
49};
50
51#ifndef RUNNING_FRC_TESTS
52int main() {
53 return frc::StartRobot<Robot>();
54}
55#endif