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.util.sendable.Sendable;
010import edu.wpi.first.util.sendable.SendableBuilder;
011import edu.wpi.first.util.sendable.SendableRegistry;
012
013/** Implements a PID control loop. */
014public class PIDController implements Sendable, AutoCloseable {
015  private static int instances;
016
017  // Factor for "proportional" control
018  private double m_kp;
019
020  // Factor for "integral" control
021  private double m_ki;
022
023  // Factor for "derivative" control
024  private double m_kd;
025
026  // The error range where "integral" control applies
027  private double m_iZone = Double.POSITIVE_INFINITY;
028
029  // The period (in seconds) of the loop that calls the controller
030  private final double m_period;
031
032  private double m_maximumIntegral = 1.0;
033
034  private double m_minimumIntegral = -1.0;
035
036  private double m_maximumInput;
037
038  private double m_minimumInput;
039
040  // Do the endpoints wrap around? e.g. Absolute encoder
041  private boolean m_continuous;
042
043  // The error at the time of the most recent call to calculate()
044  private double m_error;
045  private double m_errorDerivative;
046
047  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
048  private double m_prevError;
049
050  // The sum of the errors for use in the integral calc
051  private double m_totalError;
052
053  // The error that is considered at setpoint.
054  private double m_errorTolerance = 0.05;
055  private double m_errorDerivativeTolerance = Double.POSITIVE_INFINITY;
056
057  private double m_setpoint;
058  private double m_measurement;
059
060  private boolean m_haveMeasurement;
061  private boolean m_haveSetpoint;
062
063  /**
064   * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
065   * 0.02 seconds.
066   *
067   * @param kp The proportional coefficient.
068   * @param ki The integral coefficient.
069   * @param kd The derivative coefficient.
070   * @throws IllegalArgumentException if kp < 0
071   * @throws IllegalArgumentException if ki < 0
072   * @throws IllegalArgumentException if kd < 0
073   */
074  public PIDController(double kp, double ki, double kd) {
075    this(kp, ki, kd, 0.02);
076  }
077
078  /**
079   * Allocates a PIDController with the given constants for kp, ki, and kd.
080   *
081   * @param kp The proportional coefficient.
082   * @param ki The integral coefficient.
083   * @param kd The derivative coefficient.
084   * @param period The period between controller updates in seconds.
085   * @throws IllegalArgumentException if kp < 0
086   * @throws IllegalArgumentException if ki < 0
087   * @throws IllegalArgumentException if kd < 0
088   * @throws IllegalArgumentException if period <= 0
089   */
090  @SuppressWarnings("this-escape")
091  public PIDController(double kp, double ki, double kd, double period) {
092    m_kp = kp;
093    m_ki = ki;
094    m_kd = kd;
095
096    if (kp < 0.0) {
097      throw new IllegalArgumentException("Kp must be a non-negative number!");
098    }
099    if (ki < 0.0) {
100      throw new IllegalArgumentException("Ki must be a non-negative number!");
101    }
102    if (kd < 0.0) {
103      throw new IllegalArgumentException("Kd must be a non-negative number!");
104    }
105    if (period <= 0.0) {
106      throw new IllegalArgumentException("Controller period must be a positive number!");
107    }
108    m_period = period;
109
110    instances++;
111    SendableRegistry.add(this, "PIDController", instances);
112
113    MathSharedStore.reportUsage("PIDController", String.valueOf(instances));
114  }
115
116  @Override
117  public void close() {
118    SendableRegistry.remove(this);
119  }
120
121  /**
122   * Sets the PID Controller gain parameters.
123   *
124   * <p>Set the proportional, integral, and differential coefficients.
125   *
126   * @param kp The proportional coefficient.
127   * @param ki The integral coefficient.
128   * @param kd The derivative coefficient.
129   */
130  public void setPID(double kp, double ki, double kd) {
131    m_kp = kp;
132    m_ki = ki;
133    m_kd = kd;
134  }
135
136  /**
137   * Sets the Proportional coefficient of the PID controller gain.
138   *
139   * @param kp The proportional coefficient. Must be &gt;= 0.
140   */
141  public void setP(double kp) {
142    m_kp = kp;
143  }
144
145  /**
146   * Sets the Integral coefficient of the PID controller gain.
147   *
148   * @param ki The integral coefficient. Must be &gt;= 0.
149   */
150  public void setI(double ki) {
151    m_ki = ki;
152  }
153
154  /**
155   * Sets the Differential coefficient of the PID controller gain.
156   *
157   * @param kd The differential coefficient. Must be &gt;= 0.
158   */
159  public void setD(double kd) {
160    m_kd = kd;
161  }
162
163  /**
164   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
165   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
166   * the position error is less than IZone. This is used to prevent integral windup. Must be
167   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
168   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
169   *
170   * @param iZone Maximum magnitude of error to allow integral control.
171   * @throws IllegalArgumentException if iZone &lt; 0
172   */
173  public void setIZone(double iZone) {
174    if (iZone < 0) {
175      throw new IllegalArgumentException("IZone must be a non-negative number!");
176    }
177    m_iZone = iZone;
178  }
179
180  /**
181   * Get the Proportional coefficient.
182   *
183   * @return proportional coefficient
184   */
185  public double getP() {
186    return m_kp;
187  }
188
189  /**
190   * Get the Integral coefficient.
191   *
192   * @return integral coefficient
193   */
194  public double getI() {
195    return m_ki;
196  }
197
198  /**
199   * Get the Differential coefficient.
200   *
201   * @return differential coefficient
202   */
203  public double getD() {
204    return m_kd;
205  }
206
207  /**
208   * Get the IZone range.
209   *
210   * @return Maximum magnitude of error to allow integral control.
211   */
212  public double getIZone() {
213    return m_iZone;
214  }
215
216  /**
217   * Returns the period of this controller.
218   *
219   * @return the period of the controller.
220   */
221  public double getPeriod() {
222    return m_period;
223  }
224
225  /**
226   * Returns the position tolerance of this controller.
227   *
228   * @return the position tolerance of the controller.
229   * @deprecated Use getErrorTolerance() instead.
230   */
231  @Deprecated(forRemoval = true, since = "2025")
232  public double getPositionTolerance() {
233    return m_errorTolerance;
234  }
235
236  /**
237   * Returns the velocity tolerance of this controller.
238   *
239   * @return the velocity tolerance of the controller.
240   * @deprecated Use getErrorDerivativeTolerance() instead.
241   */
242  @Deprecated(forRemoval = true, since = "2025")
243  public double getVelocityTolerance() {
244    return m_errorDerivativeTolerance;
245  }
246
247  /**
248   * Returns the error tolerance of this controller. Defaults to 0.05.
249   *
250   * @return the error tolerance of the controller.
251   */
252  public double getErrorTolerance() {
253    return m_errorTolerance;
254  }
255
256  /**
257   * Returns the error derivative tolerance of this controller. Defaults to ∞.
258   *
259   * @return the error derivative tolerance of the controller.
260   */
261  public double getErrorDerivativeTolerance() {
262    return m_errorDerivativeTolerance;
263  }
264
265  /**
266   * Returns the accumulated error used in the integral calculation of this controller.
267   *
268   * @return The accumulated error of this controller.
269   */
270  public double getAccumulatedError() {
271    return m_totalError;
272  }
273
274  /**
275   * Sets the setpoint for the PIDController.
276   *
277   * @param setpoint The desired setpoint.
278   */
279  public void setSetpoint(double setpoint) {
280    m_setpoint = setpoint;
281    m_haveSetpoint = true;
282
283    if (m_continuous) {
284      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
285      m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
286    } else {
287      m_error = m_setpoint - m_measurement;
288    }
289
290    m_errorDerivative = (m_error - m_prevError) / m_period;
291  }
292
293  /**
294   * Returns the current setpoint of the PIDController.
295   *
296   * @return The current setpoint.
297   */
298  public double getSetpoint() {
299    return m_setpoint;
300  }
301
302  /**
303   * Returns true if the error is within the tolerance of the setpoint. The error tolerance defaults
304   * to 0.05, and the error derivative tolerance defaults to ∞.
305   *
306   * <p>This will return false until at least one input value has been computed.
307   *
308   * @return Whether the error is within the acceptable bounds.
309   */
310  public boolean atSetpoint() {
311    return m_haveMeasurement
312        && m_haveSetpoint
313        && Math.abs(m_error) < m_errorTolerance
314        && Math.abs(m_errorDerivative) < m_errorDerivativeTolerance;
315  }
316
317  /**
318   * Enables continuous input.
319   *
320   * <p>Rather then using the max and min input range as constraints, it considers them to be the
321   * same point and automatically calculates the shortest route to the setpoint.
322   *
323   * @param minimumInput The minimum value expected from the input.
324   * @param maximumInput The maximum value expected from the input.
325   */
326  public void enableContinuousInput(double minimumInput, double maximumInput) {
327    m_continuous = true;
328    m_minimumInput = minimumInput;
329    m_maximumInput = maximumInput;
330  }
331
332  /** Disables continuous input. */
333  public void disableContinuousInput() {
334    m_continuous = false;
335  }
336
337  /**
338   * Returns true if continuous input is enabled.
339   *
340   * @return True if continuous input is enabled.
341   */
342  public boolean isContinuousInputEnabled() {
343    return m_continuous;
344  }
345
346  /**
347   * Sets the minimum and maximum contributions of the integral term.
348   *
349   * <p>The internal integrator is clamped so that the integral term's contribution to the output
350   * stays between minimumIntegral and maximumIntegral. This prevents integral windup.
351   *
352   * @param minimumIntegral The minimum contribution of the integral term.
353   * @param maximumIntegral The maximum contribution of the integral term.
354   */
355  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
356    m_minimumIntegral = minimumIntegral;
357    m_maximumIntegral = maximumIntegral;
358  }
359
360  /**
361   * Sets the error which is considered tolerable for use with atSetpoint().
362   *
363   * @param errorTolerance Error which is tolerable.
364   */
365  public void setTolerance(double errorTolerance) {
366    setTolerance(errorTolerance, Double.POSITIVE_INFINITY);
367  }
368
369  /**
370   * Sets the error which is considered tolerable for use with atSetpoint().
371   *
372   * @param errorTolerance Error which is tolerable.
373   * @param errorDerivativeTolerance Error derivative which is tolerable.
374   */
375  public void setTolerance(double errorTolerance, double errorDerivativeTolerance) {
376    m_errorTolerance = errorTolerance;
377    m_errorDerivativeTolerance = errorDerivativeTolerance;
378  }
379
380  /**
381   * Returns the difference between the setpoint and the measurement.
382   *
383   * @return The error.
384   * @deprecated Use getError() instead.
385   */
386  @Deprecated(forRemoval = true, since = "2025")
387  public double getPositionError() {
388    return m_error;
389  }
390
391  /**
392   * Returns the velocity error.
393   *
394   * @return The velocity error.
395   * @deprecated Use getErrorDerivative() instead.
396   */
397  @Deprecated(forRemoval = true, since = "2025")
398  public double getVelocityError() {
399    return m_errorDerivative;
400  }
401
402  /**
403   * Returns the difference between the setpoint and the measurement.
404   *
405   * @return The error.
406   */
407  public double getError() {
408    return m_error;
409  }
410
411  /**
412   * Returns the error derivative.
413   *
414   * @return The error derivative.
415   */
416  public double getErrorDerivative() {
417    return m_errorDerivative;
418  }
419
420  /**
421   * Returns the next output of the PID controller.
422   *
423   * @param measurement The current measurement of the process variable.
424   * @param setpoint The new setpoint of the controller.
425   * @return The next controller output.
426   */
427  public double calculate(double measurement, double setpoint) {
428    m_setpoint = setpoint;
429    m_haveSetpoint = true;
430    return calculate(measurement);
431  }
432
433  /**
434   * Returns the next output of the PID controller.
435   *
436   * @param measurement The current measurement of the process variable.
437   * @return The next controller output.
438   */
439  public double calculate(double measurement) {
440    m_measurement = measurement;
441    m_prevError = m_error;
442    m_haveMeasurement = true;
443
444    if (m_continuous) {
445      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
446      m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
447    } else {
448      m_error = m_setpoint - m_measurement;
449    }
450
451    m_errorDerivative = (m_error - m_prevError) / m_period;
452
453    // If the absolute value of the position error is greater than IZone, reset the total error
454    if (Math.abs(m_error) > m_iZone) {
455      m_totalError = 0;
456    } else if (m_ki != 0) {
457      m_totalError =
458          MathUtil.clamp(
459              m_totalError + m_error * m_period,
460              m_minimumIntegral / m_ki,
461              m_maximumIntegral / m_ki);
462    }
463
464    return m_kp * m_error + m_ki * m_totalError + m_kd * m_errorDerivative;
465  }
466
467  /** Resets the previous error and the integral term. */
468  public void reset() {
469    m_error = 0;
470    m_prevError = 0;
471    m_totalError = 0;
472    m_errorDerivative = 0;
473    m_haveMeasurement = false;
474  }
475
476  @Override
477  public void initSendable(SendableBuilder builder) {
478    builder.setSmartDashboardType("PIDController");
479    builder.addDoubleProperty("p", this::getP, this::setP);
480    builder.addDoubleProperty("i", this::getI, this::setI);
481    builder.addDoubleProperty("d", this::getD, this::setD);
482    builder.addDoubleProperty(
483        "izone",
484        this::getIZone,
485        (double toSet) -> {
486          try {
487            setIZone(toSet);
488          } catch (IllegalArgumentException e) {
489            MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace());
490          }
491        });
492    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
493    builder.addDoubleProperty("measurement", () -> m_measurement, null);
494    builder.addDoubleProperty("error", this::getError, null);
495    builder.addDoubleProperty("error derivative", this::getErrorDerivative, null);
496    builder.addDoubleProperty("previous error", () -> this.m_prevError, null);
497    builder.addDoubleProperty("total error", this::getAccumulatedError, null);
498  }
499}