001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.controller;
006
007import edu.wpi.first.math.MathSharedStore;
008import edu.wpi.first.math.MathUtil;
009import edu.wpi.first.math.trajectory.TrapezoidProfile;
010import edu.wpi.first.util.sendable.Sendable;
011import edu.wpi.first.util.sendable.SendableBuilder;
012import edu.wpi.first.util.sendable.SendableRegistry;
013
014/**
015 * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
016 * call reset() when they first start running the controller to avoid unwanted behavior.
017 */
018public class ProfiledPIDController implements Sendable {
019  private static int instances;
020
021  private PIDController m_controller;
022  private double m_minimumInput;
023  private double m_maximumInput;
024
025  private TrapezoidProfile.Constraints m_constraints;
026  private TrapezoidProfile m_profile;
027  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
028  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
029
030  /**
031   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
032   *
033   * @param Kp The proportional coefficient.
034   * @param Ki The integral coefficient.
035   * @param Kd The derivative coefficient.
036   * @param constraints Velocity and acceleration constraints for goal.
037   * @throws IllegalArgumentException if kp < 0
038   * @throws IllegalArgumentException if ki < 0
039   * @throws IllegalArgumentException if kd < 0
040   */
041  public ProfiledPIDController(
042      double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
043    this(Kp, Ki, Kd, constraints, 0.02);
044  }
045
046  /**
047   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
048   *
049   * @param Kp The proportional coefficient.
050   * @param Ki The integral coefficient.
051   * @param Kd The derivative coefficient.
052   * @param constraints Velocity and acceleration constraints for goal.
053   * @param period The period between controller updates in seconds. The default is 0.02 seconds.
054   * @throws IllegalArgumentException if kp < 0
055   * @throws IllegalArgumentException if ki < 0
056   * @throws IllegalArgumentException if kd < 0
057   * @throws IllegalArgumentException if period <= 0
058   */
059  @SuppressWarnings("this-escape")
060  public ProfiledPIDController(
061      double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
062    m_controller = new PIDController(Kp, Ki, Kd, period);
063    m_constraints = constraints;
064    m_profile = new TrapezoidProfile(m_constraints);
065    instances++;
066
067    SendableRegistry.add(this, "ProfiledPIDController", instances);
068    MathSharedStore.reportUsage("ProfiledPIDController", String.valueOf(instances));
069  }
070
071  /**
072   * Sets the PID Controller gain parameters.
073   *
074   * <p>Sets the proportional, integral, and differential coefficients.
075   *
076   * @param Kp The proportional coefficient. Must be &gt;= 0.
077   * @param Ki The integral coefficient. Must be &gt;= 0.
078   * @param Kd The differential coefficient. Must be &gt;= 0.
079   */
080  public void setPID(double Kp, double Ki, double Kd) {
081    m_controller.setPID(Kp, Ki, Kd);
082  }
083
084  /**
085   * Sets the proportional coefficient of the PID controller gain.
086   *
087   * @param Kp The proportional coefficient. Must be &gt;= 0.
088   */
089  public void setP(double Kp) {
090    m_controller.setP(Kp);
091  }
092
093  /**
094   * Sets the integral coefficient of the PID controller gain.
095   *
096   * @param Ki The integral coefficient. Must be &gt;= 0.
097   */
098  public void setI(double Ki) {
099    m_controller.setI(Ki);
100  }
101
102  /**
103   * Sets the differential coefficient of the PID controller gain.
104   *
105   * @param Kd The differential coefficient. Must be &gt;= 0.
106   */
107  public void setD(double Kd) {
108    m_controller.setD(Kd);
109  }
110
111  /**
112   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
113   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
114   * the position error is less than IZone. This is used to prevent integral windup. Must be
115   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
116   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
117   *
118   * @param iZone Maximum magnitude of error to allow integral control.
119   * @throws IllegalArgumentException if iZone &lt;= 0
120   */
121  public void setIZone(double iZone) {
122    m_controller.setIZone(iZone);
123  }
124
125  /**
126   * Gets the proportional coefficient.
127   *
128   * @return proportional coefficient
129   */
130  public double getP() {
131    return m_controller.getP();
132  }
133
134  /**
135   * Gets the integral coefficient.
136   *
137   * @return integral coefficient
138   */
139  public double getI() {
140    return m_controller.getI();
141  }
142
143  /**
144   * Gets the differential coefficient.
145   *
146   * @return differential coefficient
147   */
148  public double getD() {
149    return m_controller.getD();
150  }
151
152  /**
153   * Get the IZone range.
154   *
155   * @return Maximum magnitude of error to allow integral control.
156   */
157  public double getIZone() {
158    return m_controller.getIZone();
159  }
160
161  /**
162   * Gets the period of this controller.
163   *
164   * @return The period of the controller.
165   */
166  public double getPeriod() {
167    return m_controller.getPeriod();
168  }
169
170  /**
171   * Returns the position tolerance of this controller.
172   *
173   * @return the position tolerance of the controller.
174   */
175  public double getPositionTolerance() {
176    return m_controller.getErrorTolerance();
177  }
178
179  /**
180   * Returns the velocity tolerance of this controller.
181   *
182   * @return the velocity tolerance of the controller.
183   */
184  public double getVelocityTolerance() {
185    return m_controller.getErrorDerivativeTolerance();
186  }
187
188  /**
189   * Returns the accumulated error used in the integral calculation of this controller.
190   *
191   * @return The accumulated error of this controller.
192   */
193  public double getAccumulatedError() {
194    return m_controller.getAccumulatedError();
195  }
196
197  /**
198   * Sets the goal for the ProfiledPIDController.
199   *
200   * @param goal The desired goal state.
201   */
202  public void setGoal(TrapezoidProfile.State goal) {
203    m_goal = goal;
204  }
205
206  /**
207   * Sets the goal for the ProfiledPIDController.
208   *
209   * @param goal The desired goal position.
210   */
211  public void setGoal(double goal) {
212    m_goal = new TrapezoidProfile.State(goal, 0);
213  }
214
215  /**
216   * Gets the goal for the ProfiledPIDController.
217   *
218   * @return The goal.
219   */
220  public TrapezoidProfile.State getGoal() {
221    return m_goal;
222  }
223
224  /**
225   * Returns true if the error is within the tolerance of the error.
226   *
227   * <p>This will return false until at least one input value has been computed.
228   *
229   * @return True if the error is within the tolerance of the error.
230   */
231  public boolean atGoal() {
232    return atSetpoint() && m_goal.equals(m_setpoint);
233  }
234
235  /**
236   * Set velocity and acceleration constraints for goal.
237   *
238   * @param constraints Velocity and acceleration constraints for goal.
239   */
240  public void setConstraints(TrapezoidProfile.Constraints constraints) {
241    m_constraints = constraints;
242    m_profile = new TrapezoidProfile(m_constraints);
243  }
244
245  /**
246   * Get the velocity and acceleration constraints for this controller.
247   *
248   * @return Velocity and acceleration constraints.
249   */
250  public TrapezoidProfile.Constraints getConstraints() {
251    return m_constraints;
252  }
253
254  /**
255   * Returns the current setpoint of the ProfiledPIDController.
256   *
257   * @return The current setpoint.
258   */
259  public TrapezoidProfile.State getSetpoint() {
260    return m_setpoint;
261  }
262
263  /**
264   * Returns true if the error is within the tolerance of the error.
265   *
266   * <p>This will return false until at least one input value has been computed.
267   *
268   * @return True if the error is within the tolerance of the error.
269   */
270  public boolean atSetpoint() {
271    return m_controller.atSetpoint();
272  }
273
274  /**
275   * Enables continuous input.
276   *
277   * <p>Rather then using the max and min input range as constraints, it considers them to be the
278   * same point and automatically calculates the shortest route to the setpoint.
279   *
280   * @param minimumInput The minimum value expected from the input.
281   * @param maximumInput The maximum value expected from the input.
282   */
283  public void enableContinuousInput(double minimumInput, double maximumInput) {
284    m_controller.enableContinuousInput(minimumInput, maximumInput);
285    m_minimumInput = minimumInput;
286    m_maximumInput = maximumInput;
287  }
288
289  /** Disables continuous input. */
290  public void disableContinuousInput() {
291    m_controller.disableContinuousInput();
292  }
293
294  /**
295   * Sets the minimum and maximum contributions of the integral term.
296   *
297   * <p>The internal integrator is clamped so that the integral term's contribution to the output
298   * stays between minimumIntegral and maximumIntegral. This prevents integral windup.
299   *
300   * @param minimumIntegral The minimum contribution of the integral term.
301   * @param maximumIntegral The maximum contribution of the integral term.
302   */
303  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
304    m_controller.setIntegratorRange(minimumIntegral, maximumIntegral);
305  }
306
307  /**
308   * Sets the error which is considered tolerable for use with atSetpoint().
309   *
310   * @param positionTolerance Position error which is tolerable.
311   */
312  public void setTolerance(double positionTolerance) {
313    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
314  }
315
316  /**
317   * Sets the error which is considered tolerable for use with atSetpoint().
318   *
319   * @param positionTolerance Position error which is tolerable.
320   * @param velocityTolerance Velocity error which is tolerable.
321   */
322  public void setTolerance(double positionTolerance, double velocityTolerance) {
323    m_controller.setTolerance(positionTolerance, velocityTolerance);
324  }
325
326  /**
327   * Returns the difference between the setpoint and the measurement.
328   *
329   * @return The error.
330   */
331  public double getPositionError() {
332    return m_controller.getError();
333  }
334
335  /**
336   * Returns the change in error per second.
337   *
338   * @return The change in error per second.
339   */
340  public double getVelocityError() {
341    return m_controller.getErrorDerivative();
342  }
343
344  /**
345   * Returns the next output of the PID controller.
346   *
347   * @param measurement The current measurement of the process variable.
348   * @return The controller's next output.
349   */
350  public double calculate(double measurement) {
351    if (m_controller.isContinuousInputEnabled()) {
352      // Get error which is the smallest distance between goal and measurement
353      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
354      double goalMinDistance =
355          MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound);
356      double setpointMinDistance =
357          MathUtil.inputModulus(m_setpoint.position - measurement, -errorBound, errorBound);
358
359      // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal
360      // may be outside the input range after this operation, but that's OK because the controller
361      // will still go there and report an error of zero. In other words, the setpoint only needs to
362      // be offset from the measurement by the input range modulus; they don't need to be equal.
363      m_goal.position = goalMinDistance + measurement;
364      m_setpoint.position = setpointMinDistance + measurement;
365    }
366
367    m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal);
368    return m_controller.calculate(measurement, m_setpoint.position);
369  }
370
371  /**
372   * Returns the next output of the PID controller.
373   *
374   * @param measurement The current measurement of the process variable.
375   * @param goal The new goal of the controller.
376   * @return The controller's next output.
377   */
378  public double calculate(double measurement, TrapezoidProfile.State goal) {
379    setGoal(goal);
380    return calculate(measurement);
381  }
382
383  /**
384   * Returns the next output of the PIDController.
385   *
386   * @param measurement The current measurement of the process variable.
387   * @param goal The new goal of the controller.
388   * @return The controller's next output.
389   */
390  public double calculate(double measurement, double goal) {
391    setGoal(goal);
392    return calculate(measurement);
393  }
394
395  /**
396   * Returns the next output of the PID controller.
397   *
398   * @param measurement The current measurement of the process variable.
399   * @param goal The new goal of the controller.
400   * @param constraints Velocity and acceleration constraints for goal.
401   * @return The controller's next output.
402   */
403  public double calculate(
404      double measurement, TrapezoidProfile.State goal, TrapezoidProfile.Constraints constraints) {
405    setConstraints(constraints);
406    return calculate(measurement, goal);
407  }
408
409  /**
410   * Reset the previous error and the integral term.
411   *
412   * @param measurement The current measured State of the system.
413   */
414  public void reset(TrapezoidProfile.State measurement) {
415    m_controller.reset();
416    m_setpoint = measurement;
417  }
418
419  /**
420   * Reset the previous error and the integral term.
421   *
422   * @param measuredPosition The current measured position of the system.
423   * @param measuredVelocity The current measured velocity of the system.
424   */
425  public void reset(double measuredPosition, double measuredVelocity) {
426    reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
427  }
428
429  /**
430   * Reset the previous error and the integral term.
431   *
432   * @param measuredPosition The current measured position of the system. The velocity is assumed to
433   *     be zero.
434   */
435  public void reset(double measuredPosition) {
436    reset(measuredPosition, 0.0);
437  }
438
439  @Override
440  public void initSendable(SendableBuilder builder) {
441    builder.setSmartDashboardType("ProfiledPIDController");
442    builder.addDoubleProperty("p", this::getP, this::setP);
443    builder.addDoubleProperty("i", this::getI, this::setI);
444    builder.addDoubleProperty("d", this::getD, this::setD);
445    builder.addDoubleProperty(
446        "izone",
447        this::getIZone,
448        (double toSet) -> {
449          try {
450            setIZone(toSet);
451          } catch (IllegalArgumentException e) {
452            MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace());
453          }
454        });
455    builder.addDoubleProperty(
456        "maxVelocity",
457        () -> getConstraints().maxVelocity,
458        maxVelocity ->
459            setConstraints(
460                new TrapezoidProfile.Constraints(maxVelocity, getConstraints().maxAcceleration)));
461    builder.addDoubleProperty(
462        "maxAcceleration",
463        () -> getConstraints().maxAcceleration,
464        maxAcceleration ->
465            setConstraints(
466                new TrapezoidProfile.Constraints(getConstraints().maxVelocity, maxAcceleration)));
467    builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
468  }
469}