Source code

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.MathUsageId;
009import edu.wpi.first.math.MathUtil;
010import edu.wpi.first.util.sendable.Sendable;
011import edu.wpi.first.util.sendable.SendableBuilder;
012import edu.wpi.first.util.sendable.SendableRegistry;
013
014/** Implements a PID control loop. */
015public class PIDController implements Sendable, AutoCloseable {
016  private static int instances;
017
018  // Factor for "proportional" control
019  private double m_kp;
020
021  // Factor for "integral" control
022  private double m_ki;
023
024  // Factor for "derivative" control
025  private double m_kd;
026
027  // The period (in seconds) of the loop that calls the controller
028  private final double m_period;
029
030  private double m_maximumIntegral = 1.0;
031
032  private double m_minimumIntegral = -1.0;
033
034  private double m_maximumInput;
035
036  private double m_minimumInput;
037
038  // Do the endpoints wrap around? e.g. Absolute encoder
039  private boolean m_continuous;
040
041  // The error at the time of the most recent call to calculate()
042  private double m_positionError;
043  private double m_velocityError;
044
045  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
046  private double m_prevError;
047
048  // The sum of the errors for use in the integral calc
049  private double m_totalError;
050
051  // The error that is considered at setpoint.
052  private double m_positionTolerance = 0.05;
053  private double m_velocityTolerance = Double.POSITIVE_INFINITY;
054
055  private double m_setpoint;
056  private double m_measurement;
057
058  private boolean m_haveMeasurement;
059  private boolean m_haveSetpoint;
060
061  /**
062   * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
063   * 0.02 seconds.
064   *
065   * @param kp The proportional coefficient.
066   * @param ki The integral coefficient.
067   * @param kd The derivative coefficient.
068   */
069  public PIDController(double kp, double ki, double kd) {
070    this(kp, ki, kd, 0.02);
071  }
072
073  /**
074   * Allocates a PIDController with the given constants for kp, ki, and kd.
075   *
076   * @param kp The proportional coefficient.
077   * @param ki The integral coefficient.
078   * @param kd The derivative coefficient.
079   * @param period The period between controller updates in seconds. Must be non-zero and positive.
080   */
081  public PIDController(double kp, double ki, double kd, double period) {
082    m_kp = kp;
083    m_ki = ki;
084    m_kd = kd;
085
086    if (period <= 0) {
087      throw new IllegalArgumentException("Controller period must be a non-zero positive number!");
088    }
089    m_period = period;
090
091    instances++;
092    SendableRegistry.addLW(this, "PIDController", instances);
093
094    MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
095  }
096
097  @Override
098  public void close() {
099    SendableRegistry.remove(this);
100  }
101
102  /**
103   * Sets the PID Controller gain parameters.
104   *
105   * <p>Set the proportional, integral, and differential coefficients.
106   *
107   * @param kp The proportional coefficient.
108   * @param ki The integral coefficient.
109   * @param kd The derivative coefficient.
110   */
111  public void setPID(double kp, double ki, double kd) {
112    m_kp = kp;
113    m_ki = ki;
114    m_kd = kd;
115  }
116
117  /**
118   * Sets the Proportional coefficient of the PID controller gain.
119   *
120   * @param kp proportional coefficient
121   */
122  public void setP(double kp) {
123    m_kp = kp;
124  }
125
126  /**
127   * Sets the Integral coefficient of the PID controller gain.
128   *
129   * @param ki integral coefficient
130   */
131  public void setI(double ki) {
132    m_ki = ki;
133  }
134
135  /**
136   * Sets the Differential coefficient of the PID controller gain.
137   *
138   * @param kd differential coefficient
139   */
140  public void setD(double kd) {
141    m_kd = kd;
142  }
143
144  /**
145   * Get the Proportional coefficient.
146   *
147   * @return proportional coefficient
148   */
149  public double getP() {
150    return m_kp;
151  }
152
153  /**
154   * Get the Integral coefficient.
155   *
156   * @return integral coefficient
157   */
158  public double getI() {
159    return m_ki;
160  }
161
162  /**
163   * Get the Differential coefficient.
164   *
165   * @return differential coefficient
166   */
167  public double getD() {
168    return m_kd;
169  }
170
171  /**
172   * Returns the period of this controller.
173   *
174   * @return the period of the controller.
175   */
176  public double getPeriod() {
177    return m_period;
178  }
179
180  /**
181   * Returns the position tolerance of this controller.
182   *
183   * @return the position tolerance of the controller.
184   */
185  public double getPositionTolerance() {
186    return m_positionTolerance;
187  }
188
189  /**
190   * Returns the velocity tolerance of this controller.
191   *
192   * @return the velocity tolerance of the controller.
193   */
194  public double getVelocityTolerance() {
195    return m_velocityTolerance;
196  }
197
198  /**
199   * Sets the setpoint for the PIDController.
200   *
201   * @param setpoint The desired setpoint.
202   */
203  public void setSetpoint(double setpoint) {
204    m_setpoint = setpoint;
205    m_haveSetpoint = true;
206
207    if (m_continuous) {
208      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
209      m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
210    } else {
211      m_positionError = m_setpoint - m_measurement;
212    }
213
214    m_velocityError = (m_positionError - m_prevError) / m_period;
215  }
216
217  /**
218   * Returns the current setpoint of the PIDController.
219   *
220   * @return The current setpoint.
221   */
222  public double getSetpoint() {
223    return m_setpoint;
224  }
225
226  /**
227   * Returns true if the error is within the tolerance of the setpoint.
228   *
229   * <p>This will return false until at least one input value has been computed.
230   *
231   * @return Whether the error is within the acceptable bounds.
232   */
233  public boolean atSetpoint() {
234    return m_haveMeasurement
235        && m_haveSetpoint
236        && Math.abs(m_positionError) < m_positionTolerance
237        && Math.abs(m_velocityError) < m_velocityTolerance;
238  }
239
240  /**
241   * Enables continuous input.
242   *
243   * <p>Rather then using the max and min input range as constraints, it considers them to be the
244   * same point and automatically calculates the shortest route to the setpoint.
245   *
246   * @param minimumInput The minimum value expected from the input.
247   * @param maximumInput The maximum value expected from the input.
248   */
249  public void enableContinuousInput(double minimumInput, double maximumInput) {
250    m_continuous = true;
251    m_minimumInput = minimumInput;
252    m_maximumInput = maximumInput;
253  }
254
255  /** Disables continuous input. */
256  public void disableContinuousInput() {
257    m_continuous = false;
258  }
259
260  /**
261   * Returns true if continuous input is enabled.
262   *
263   * @return True if continuous input is enabled.
264   */
265  public boolean isContinuousInputEnabled() {
266    return m_continuous;
267  }
268
269  /**
270   * Sets the minimum and maximum values for the integrator.
271   *
272   * <p>When the cap is reached, the integrator value is added to the controller output rather than
273   * the integrator value times the integral gain.
274   *
275   * @param minimumIntegral The minimum value of the integrator.
276   * @param maximumIntegral The maximum value of the integrator.
277   */
278  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
279    m_minimumIntegral = minimumIntegral;
280    m_maximumIntegral = maximumIntegral;
281  }
282
283  /**
284   * Sets the error which is considered tolerable for use with atSetpoint().
285   *
286   * @param positionTolerance Position error which is tolerable.
287   */
288  public void setTolerance(double positionTolerance) {
289    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
290  }
291
292  /**
293   * Sets the error which is considered tolerable for use with atSetpoint().
294   *
295   * @param positionTolerance Position error which is tolerable.
296   * @param velocityTolerance Velocity error which is tolerable.
297   */
298  public void setTolerance(double positionTolerance, double velocityTolerance) {
299    m_positionTolerance = positionTolerance;
300    m_velocityTolerance = velocityTolerance;
301  }
302
303  /**
304   * Returns the difference between the setpoint and the measurement.
305   *
306   * @return The error.
307   */
308  public double getPositionError() {
309    return m_positionError;
310  }
311
312  /**
313   * Returns the velocity error.
314   *
315   * @return The velocity error.
316   */
317  public double getVelocityError() {
318    return m_velocityError;
319  }
320
321  /**
322   * Returns the next output of the PID controller.
323   *
324   * @param measurement The current measurement of the process variable.
325   * @param setpoint The new setpoint of the controller.
326   * @return The next controller output.
327   */
328  public double calculate(double measurement, double setpoint) {
329    m_setpoint = setpoint;
330    m_haveSetpoint = true;
331    return calculate(measurement);
332  }
333
334  /**
335   * Returns the next output of the PID controller.
336   *
337   * @param measurement The current measurement of the process variable.
338   * @return The next controller output.
339   */
340  public double calculate(double measurement) {
341    m_measurement = measurement;
342    m_prevError = m_positionError;
343    m_haveMeasurement = true;
344
345    if (m_continuous) {
346      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
347      m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
348    } else {
349      m_positionError = m_setpoint - m_measurement;
350    }
351
352    m_velocityError = (m_positionError - m_prevError) / m_period;
353
354    if (m_ki != 0) {
355      m_totalError =
356          MathUtil.clamp(
357              m_totalError + m_positionError * m_period,
358              m_minimumIntegral / m_ki,
359              m_maximumIntegral / m_ki);
360    }
361
362    return m_kp * m_positionError + m_ki * m_totalError + m_kd * m_velocityError;
363  }
364
365  /** Resets the previous error and the integral term. */
366  public void reset() {
367    m_positionError = 0;
368    m_prevError = 0;
369    m_totalError = 0;
370    m_velocityError = 0;
371    m_haveMeasurement = false;
372  }
373
374  @Override
375  public void initSendable(SendableBuilder builder) {
376    builder.setSmartDashboardType("PIDController");
377    builder.addDoubleProperty("p", this::getP, this::setP);
378    builder.addDoubleProperty("i", this::getI, this::setI);
379    builder.addDoubleProperty("d", this::getD, this::setD);
380    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
381  }
382}