# Combining Motion Profiling and PID Control with ProfiledPIDController .. note:: For a guide on implementing the ``ProfiledPIDController`` class in the :ref:`command-based framework ` framework, see :ref:`docs/software/commandbased/commands-v2/profilepid-subsystems-commands: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](https://github.wpilib.org/allwpilib/docs/beta/java/org/wpilib/math/controller/ProfiledPIDController.html), [C++](https://github.wpilib.org/allwpilib/docs/beta/cpp/classwpi_1_1math_1_1_profiled_p_i_d_controller.html), :external:py:class:`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 :ref:`docs/software/basic-programming/cpp-units: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 :ref:`docs/software/advanced-controls/controllers/pidcontroller: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 :ref:`creating a PIDController `. The only difference is the need to supply a set of :ref:`trapezoidal profile constraints `, which will be automatically forwarded to the internally-generated ``TrapezoidProfile`` instances: .. tab-set-code:: ```java // 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)); ``` ```c++ // Creates a ProfiledPIDController // Max velocity is 5 meters per second // Max acceleration is 10 meters per second wpi::math::ProfiledPIDController controller( kP, kI, kD, wpi::math::TrapezoidProfile::Constraints{5_mps, 10_mps_sq}); ``` ```python 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: .. tab-set-code:: ```java // 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)); ``` ```c++ // 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)); ``` ```python # 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 :ref:`feedforward `). This can be done with the ``getSetpoint()`` method. The returned setpoint might then be used as in the following example: .. tab-set-code:: .. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2027.0.0-alpha-6/wpilibjExamples/src/main/java/org/wpilib/snippets/profiledpidfeedforward/Robot.java :language: java :lines: 37-42 :lineno-match: .. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2027.0.0-alpha-6/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp :language: c++ :lines: 25-32 :lineno-match: .. code-block:: python 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](https://github.com/wpilibsuite/allwpilib/tree/v2027.0.0-alpha-6/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorprofiledpid), [C++](https://github.com/wpilibsuite/allwpilib/tree/v2027.0.0-alpha-6/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp), [Python](https://github.com/robotpy/mostrobotpy/tree/main/examples/robot/ElevatorProfiledPID)): .. tab-set-code:: .. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2027.0.0-alpha-6/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorprofiledpid/Robot.java :language: java :lines: 5- :lineno-match: .. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2027.0.0-alpha-6/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp :language: c++ :lines: 5- :lineno-match: .. remoteliteralinclude:: https://raw.githubusercontent.com/robotpy/mostrobotpy/2027.0.0a6/examples/robot/ElevatorProfiledPID/robot.py :language: python :lines: 8- :lineno-match: