Combining Motion Profiling and PID Control with ProfiledPIDController

Note

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++, Python) 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

Note

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.

Note

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 Control in WPILib.

Constructing a ProfiledPIDController

Note

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
wpi::math::ProfiledPIDController<units::meters> controller(
  kP, kI, kD,
  wpi::math::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq});

from wpimath.controller import ProfiledPIDController
from wpimath.trajectory import TrapezoidProfile
  # Creates a ProfiledPIDController
# Max velocity is 5 meters per second
# Max acceleration is 10 meters per second
controller = ProfiledPIDController(
  kP, kI, kD,
  TrapezoidProfile.Constraints(5, 10))

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));

# 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:

37  public void goToPosition(double goalPosition) {
38    double pidVal = controller.calculate(encoder.getDistance(), goalPosition);
39    motor.setVoltage(
40        pidVal + feedforward.calculate(lastVelocity, controller.getSetpoint().velocity));
41    lastVelocity = controller.getSetpoint().velocity;
42  }

25  void GoToPosition(wpi::units::meter_t goalPosition) {
26    auto pidVal = controller.Calculate(
27        wpi::units::meter_t{encoder.GetDistance()}, goalPosition);
28    motor.SetVoltage(
29        wpi::units::volt_t{pidVal} +
30        feedforward.Calculate(lastVelocity, controller.GetSetpoint().velocity));
31    lastVelocity = controller.GetSetpoint().velocity;
32  }

from wpilib.controller import ProfiledPIDController
from wpilib.controller import SimpleMotorFeedforward
def __init__(self):
    # Assuming encoder, motor, controller are already defined
    self.lastSpeed = 0
    # Assuming feedforward is a SimpleMotorFeedforward object
    self.feedforward = SimpleMotorFeedforward(ks=0.0, kv=0.0, ka=0.0)
def goToPosition(self, goalPosition: float):
    pidVal = self.controller.calculate(self.encoder.getDistance(), goalPosition)
    self.motor.setVoltage(
        pidVal
        + self.feedforward.calculate(self.lastSpeed, self.controller.getSetpoint().velocity))
    self.lastSpeed = self.controller.getSetpoint().velocity

Complete Usage Example

A more complete example of ProfiledPIDController usage is provided in the ElevatorProfilePID example project (Java, C++, Python):

 5package org.wpilib.examples.elevatorprofiledpid;
 6
 7import org.wpilib.driverstation.Joystick;
 8import org.wpilib.framework.TimedRobot;
 9import org.wpilib.hardware.motor.PWMSparkMax;
10import org.wpilib.hardware.rotation.Encoder;
11import org.wpilib.math.controller.ElevatorFeedforward;
12import org.wpilib.math.controller.ProfiledPIDController;
13import org.wpilib.math.trajectory.TrapezoidProfile;
14
15public class Robot extends TimedRobot {
16  private static double kDt = 0.02;
17  private static double kMaxVelocity = 1.75;
18  private static double kMaxAcceleration = 0.75;
19  private static double kP = 1.3;
20  private static double kI = 0.0;
21  private static double kD = 0.7;
22  private static double kS = 1.1;
23  private static double kG = 1.2;
24  private static double kV = 1.3;
25
26  private final Joystick joystick = new Joystick(1);
27  private final Encoder encoder = new Encoder(1, 2);
28  private final PWMSparkMax motor = new PWMSparkMax(1);
29
30  // Create a PID controller whose setpoint's change is subject to maximum
31  // velocity and acceleration constraints.
32  private final TrapezoidProfile.Constraints constraints =
33      new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration);
34  private final ProfiledPIDController controller =
35      new ProfiledPIDController(kP, kI, kD, constraints, kDt);
36  private final ElevatorFeedforward feedforward = new ElevatorFeedforward(kS, kG, kV);
37
38  public Robot() {
39    encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
40  }
41
42  @Override
43  public void teleopPeriodic() {
44    if (joystick.getRawButtonPressed(2)) {
45      controller.setGoal(5);
46    } else if (joystick.getRawButtonPressed(3)) {
47      controller.setGoal(0);
48    }
49
50    // Run controller and update motor output
51    motor.setVoltage(
52        controller.calculate(encoder.getDistance())
53            + feedforward.calculate(controller.getSetpoint().velocity));
54  }
55}

 5#include <numbers>
 6
 7#include "wpi/driverstation/Joystick.hpp"
 8#include "wpi/framework/TimedRobot.hpp"
 9#include "wpi/hardware/motor/PWMSparkMax.hpp"
10#include "wpi/hardware/rotation/Encoder.hpp"
11#include "wpi/math/controller/ElevatorFeedforward.hpp"
12#include "wpi/math/controller/ProfiledPIDController.hpp"
13#include "wpi/math/trajectory/TrapezoidProfile.hpp"
14#include "wpi/units/acceleration.hpp"
15#include "wpi/units/length.hpp"
16#include "wpi/units/time.hpp"
17#include "wpi/units/velocity.hpp"
18#include "wpi/units/voltage.hpp"
19
20class Robot : public wpi::TimedRobot {
21 public:
22  static constexpr wpi::units::second_t kDt = 20_ms;
23
24  Robot() {
25    encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
26  }
27
28  void TeleopPeriodic() override {
29    if (joystick.GetRawButtonPressed(2)) {
30      controller.SetGoal(5_m);
31    } else if (joystick.GetRawButtonPressed(3)) {
32      controller.SetGoal(0_m);
33    }
34
35    // Run controller and update motor output
36    motor.SetVoltage(wpi::units::volt_t{controller.Calculate(
37                         wpi::units::meter_t{encoder.GetDistance()})} +
38                     feedforward.Calculate(controller.GetSetpoint().velocity));
39  }
40
41 private:
42  static constexpr wpi::units::meters_per_second_t kMaxVelocity = 1.75_mps;
43  static constexpr wpi::units::meters_per_second_squared_t kMaxAcceleration =
44      0.75_mps_sq;
45  static constexpr double kP = 1.3;
46  static constexpr double kI = 0.0;
47  static constexpr double kD = 0.7;
48  static constexpr wpi::units::volt_t kS = 1.1_V;
49  static constexpr wpi::units::volt_t kG = 1.2_V;
50  static constexpr auto kV = 1.3_V / 1_mps;
51
52  wpi::Joystick joystick{1};
53  wpi::Encoder encoder{1, 2};
54  wpi::PWMSparkMax motor{1};
55
56  // Create a PID controller whose setpoint's change is subject to maximum
57  // velocity and acceleration constraints.
58  wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints constraints{
59      kMaxVelocity, kMaxAcceleration};
60  wpi::math::ProfiledPIDController<wpi::units::meters> controller{
61      kP, kI, kD, constraints, kDt};
62  wpi::math::ElevatorFeedforward feedforward{kS, kG, kV};
63};
64
65#ifndef RUNNING_WPILIB_TESTS
66int main() {
67  return wpi::StartRobot<Robot>();
68}
69#endif

 8import math
 9
10import wpilib
11import wpimath
12
13
14class MyRobot(wpilib.TimedRobot):
15    kDt = 0.02
16    kMaxVelocity = 1.75
17    kMaxAcceleration = 0.75
18    kP = 1.3
19    kI = 0.0
20    kD = 0.7
21    kS = 1.1
22    kG = 1.2
23    kV = 1.3
24
25    def __init__(self) -> None:
26        super().__init__()
27        self.joystick = wpilib.Joystick(1)
28        self.encoder = wpilib.Encoder(1, 2)
29        self.motor = wpilib.PWMSparkMax(1)
30
31        # Create a PID controller whose setpoint's change is subject to maximum
32        # velocity and acceleration constraints.
33        self.constraints = wpimath.TrapezoidProfile.Constraints(
34            self.kMaxVelocity, self.kMaxAcceleration
35        )
36        self.controller = wpimath.ProfiledPIDController(
37            self.kP, self.kI, self.kD, self.constraints, self.kDt
38        )
39        self.feedforward = wpimath.ElevatorFeedforward(self.kS, self.kG, self.kV)
40
41        self.encoder.setDistancePerPulse(1 / 360 * 2 * math.pi * 1.5)
42
43    def teleopPeriodic(self) -> None:
44        if self.joystick.getRawButtonPressed(2):
45            self.controller.setGoal(5)
46        elif self.joystick.getRawButtonPressed(3):
47            self.controller.setGoal(0)
48
49        # Run controller and update motor output
50        self.motor.setVoltage(
51            self.controller.calculate(self.encoder.getDistance())
52            + self.feedforward.calculate(self.controller.getSetpoint().velocity)
53        )