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.wpilibj;
006
007import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.hal.FRCNetComm.tResourceType;
010import edu.wpi.first.hal.HAL;
011import edu.wpi.first.hal.util.BoundaryException;
012import edu.wpi.first.math.filter.LinearFilter;
013import edu.wpi.first.util.sendable.Sendable;
014import edu.wpi.first.util.sendable.SendableBuilder;
015import edu.wpi.first.util.sendable.SendableRegistry;
016import java.util.concurrent.locks.ReentrantLock;
017
018/**
019 * Class implements a PID Control Loop.
020 *
021 * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
022 * calculations, as well as writing the given PIDOutput.
023 *
024 * <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
025 * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
026 * given set of PID constants.
027 *
028 * <p>This class is provided by the OldCommands VendorDep
029 *
030 * @deprecated All APIs which use this have been deprecated.
031 */
032@Deprecated(since = "2020", forRemoval = true)
033public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
034  public static final double kDefaultPeriod = 0.05;
035  private static int instances;
036
037  // Factor for "proportional" control
038  @SuppressWarnings("MemberName")
039  private double m_P;
040
041  // Factor for "integral" control
042  @SuppressWarnings("MemberName")
043  private double m_I;
044
045  // Factor for "derivative" control
046  @SuppressWarnings("MemberName")
047  private double m_D;
048
049  // Factor for "feed forward" control
050  @SuppressWarnings("MemberName")
051  private double m_F;
052
053  // |maximum output|
054  private double m_maximumOutput = 1.0;
055
056  // |minimum output|
057  private double m_minimumOutput = -1.0;
058
059  // Maximum input - limit setpoint to this
060  private double m_maximumInput;
061
062  // Minimum input - limit setpoint to this
063  private double m_minimumInput;
064
065  // Input range - difference between maximum and minimum
066  private double m_inputRange;
067
068  // Do the endpoints wrap around? (e.g., absolute encoder)
069  private boolean m_continuous;
070
071  // Is the PID controller enabled
072  protected boolean m_enabled;
073
074  // The prior error (used to compute velocity)
075  private double m_prevError;
076
077  // The sum of the errors for use in the integral calc
078  private double m_totalError;
079
080  // The tolerance object used to check if on target
081  private Tolerance m_tolerance;
082
083  private double m_setpoint;
084  private double m_prevSetpoint;
085
086  private double m_result;
087
088  private LinearFilter m_filter;
089
090  protected ReentrantLock m_thisMutex = new ReentrantLock();
091
092  // Ensures when disable() is called, pidWrite() won't run if calculate()
093  // is already running at that time.
094  protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
095
096  protected PIDSource m_pidInput;
097  protected PIDOutput m_pidOutput;
098  protected Timer m_setpointTimer;
099
100  /**
101   * Tolerance is the type of tolerance used to specify if the PID controller is on target.
102   *
103   * <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
104   * specify types of tolerance specifications to use.
105   */
106  public interface Tolerance {
107    boolean onTarget();
108  }
109
110  /** Used internally for when Tolerance hasn't been set. */
111  public static class NullTolerance implements Tolerance {
112    @Override
113    public boolean onTarget() {
114      throw new IllegalStateException("No tolerance value set when calling onTarget().");
115    }
116  }
117
118  public class PercentageTolerance implements Tolerance {
119    private final double m_percentage;
120
121    PercentageTolerance(double value) {
122      m_percentage = value;
123    }
124
125    @Override
126    public boolean onTarget() {
127      return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
128    }
129  }
130
131  public class AbsoluteTolerance implements Tolerance {
132    private final double m_value;
133
134    AbsoluteTolerance(double value) {
135      m_value = value;
136    }
137
138    @Override
139    public boolean onTarget() {
140      return Math.abs(getError()) < m_value;
141    }
142  }
143
144  /**
145   * Allocate a PID object with the given constants for P, I, D, and F.
146   *
147   * @param Kp the proportional coefficient
148   * @param Ki the integral coefficient
149   * @param Kd the derivative coefficient
150   * @param Kf the feed forward term
151   * @param source The PIDSource object that is used to get values
152   * @param output The PIDOutput object that is set to the output percentage
153   */
154  @SuppressWarnings("ParameterName")
155  public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
156    requireNonNullParam(source, "source", "PIDBase");
157    requireNonNullParam(output, "output", "PIDBase");
158
159    m_setpointTimer = new Timer();
160    m_setpointTimer.start();
161
162    m_P = Kp;
163    m_I = Ki;
164    m_D = Kd;
165    m_F = Kf;
166
167    m_pidInput = source;
168    m_filter = LinearFilter.movingAverage(1);
169
170    m_pidOutput = output;
171
172    instances++;
173    HAL.report(tResourceType.kResourceType_PIDController, instances);
174    m_tolerance = new NullTolerance();
175    SendableRegistry.add(this, "PIDController", instances);
176  }
177
178  /**
179   * Allocate a PID object with the given constants for P, I, and D.
180   *
181   * @param Kp the proportional coefficient
182   * @param Ki the integral coefficient
183   * @param Kd the derivative coefficient
184   * @param source the PIDSource object that is used to get values
185   * @param output the PIDOutput object that is set to the output percentage
186   */
187  @SuppressWarnings("ParameterName")
188  public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
189    this(Kp, Ki, Kd, 0.0, source, output);
190  }
191
192  @Override
193  public void close() {
194    SendableRegistry.remove(this);
195  }
196
197  /**
198   * Read the input, calculate the output accordingly, and write to the output. This should only be
199   * called by the PIDTask and is created during initialization.
200   */
201  @SuppressWarnings("LocalVariableName")
202  protected void calculate() {
203    if (m_pidInput == null || m_pidOutput == null) {
204      return;
205    }
206
207    boolean enabled;
208
209    m_thisMutex.lock();
210    try {
211      enabled = m_enabled;
212    } finally {
213      m_thisMutex.unlock();
214    }
215
216    if (enabled) {
217      double input;
218
219      // Storage for function inputs
220      PIDSourceType pidSourceType;
221      double P;
222      double I;
223      double D;
224      double feedForward = calculateFeedForward();
225      double minimumOutput;
226      double maximumOutput;
227
228      // Storage for function input-outputs
229      double prevError;
230      double error;
231      double totalError;
232
233      m_thisMutex.lock();
234      try {
235        input = m_filter.calculate(m_pidInput.pidGet());
236
237        pidSourceType = m_pidInput.getPIDSourceType();
238        P = m_P;
239        I = m_I;
240        D = m_D;
241        minimumOutput = m_minimumOutput;
242        maximumOutput = m_maximumOutput;
243
244        prevError = m_prevError;
245        error = getContinuousError(m_setpoint - input);
246        totalError = m_totalError;
247      } finally {
248        m_thisMutex.unlock();
249      }
250
251      // Storage for function outputs
252      double result;
253
254      if (pidSourceType.equals(PIDSourceType.kRate)) {
255        if (P != 0) {
256          totalError = clamp(totalError + error, minimumOutput / P, maximumOutput / P);
257        }
258
259        result = P * totalError + D * error + feedForward;
260      } else {
261        if (I != 0) {
262          totalError = clamp(totalError + error, minimumOutput / I, maximumOutput / I);
263        }
264
265        result = P * error + I * totalError + D * (error - prevError) + feedForward;
266      }
267
268      result = clamp(result, minimumOutput, maximumOutput);
269
270      // Ensures m_enabled check and pidWrite() call occur atomically
271      m_pidWriteMutex.lock();
272      m_thisMutex.lock();
273      try {
274        if (m_enabled) {
275          // Don't block other PIDController operations on pidWrite()
276          m_thisMutex.unlock();
277
278          m_pidOutput.pidWrite(result);
279        }
280      } finally {
281        if (!m_enabled) {
282          m_thisMutex.unlock();
283        }
284        m_pidWriteMutex.unlock();
285      }
286
287      m_thisMutex.lock();
288      try {
289        m_prevError = error;
290        m_totalError = totalError;
291        m_result = result;
292      } finally {
293        m_thisMutex.unlock();
294      }
295    }
296  }
297
298  /**
299   * Calculate the feed forward term.
300   *
301   * <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
302   * feed forward calculation is desired, the user can override this function and provide his or her
303   * own. This function does no synchronization because the PIDController class only calls it in
304   * synchronized code, so be careful if calling it oneself.
305   *
306   * <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
307   * setpoint for the output. If a position PID controller is being used, the F term should be set
308   * to 1 over the maximum speed for the output measured in setpoint units per this controller's
309   * update period (see the default period in this class's constructor).
310   *
311   * @return The feedforward value.
312   */
313  protected double calculateFeedForward() {
314    if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
315      return m_F * getSetpoint();
316    } else {
317      double temp = m_F * getDeltaSetpoint();
318      m_prevSetpoint = m_setpoint;
319      m_setpointTimer.reset();
320      return temp;
321    }
322  }
323
324  /**
325   * Set the PID Controller gain parameters. Set the proportional, integral, and differential
326   * coefficients.
327   *
328   * @param p Proportional coefficient
329   * @param i Integral coefficient
330   * @param d Differential coefficient
331   */
332  @Override
333  @SuppressWarnings("ParameterName")
334  public void setPID(double p, double i, double d) {
335    m_thisMutex.lock();
336    try {
337      m_P = p;
338      m_I = i;
339      m_D = d;
340    } finally {
341      m_thisMutex.unlock();
342    }
343  }
344
345  /**
346   * Set the PID Controller gain parameters. Set the proportional, integral, and differential
347   * coefficients.
348   *
349   * @param p Proportional coefficient
350   * @param i Integral coefficient
351   * @param d Differential coefficient
352   * @param f Feed forward coefficient
353   */
354  @SuppressWarnings("ParameterName")
355  public void setPID(double p, double i, double d, double f) {
356    m_thisMutex.lock();
357    try {
358      m_P = p;
359      m_I = i;
360      m_D = d;
361      m_F = f;
362    } finally {
363      m_thisMutex.unlock();
364    }
365  }
366
367  /**
368   * Set the Proportional coefficient of the PID controller gain.
369   *
370   * @param p Proportional coefficient
371   */
372  @SuppressWarnings("ParameterName")
373  public void setP(double p) {
374    m_thisMutex.lock();
375    try {
376      m_P = p;
377    } finally {
378      m_thisMutex.unlock();
379    }
380  }
381
382  /**
383   * Set the Integral coefficient of the PID controller gain.
384   *
385   * @param i Integral coefficient
386   */
387  @SuppressWarnings("ParameterName")
388  public void setI(double i) {
389    m_thisMutex.lock();
390    try {
391      m_I = i;
392    } finally {
393      m_thisMutex.unlock();
394    }
395  }
396
397  /**
398   * Set the Differential coefficient of the PID controller gain.
399   *
400   * @param d differential coefficient
401   */
402  @SuppressWarnings("ParameterName")
403  public void setD(double d) {
404    m_thisMutex.lock();
405    try {
406      m_D = d;
407    } finally {
408      m_thisMutex.unlock();
409    }
410  }
411
412  /**
413   * Set the Feed forward coefficient of the PID controller gain.
414   *
415   * @param f feed forward coefficient
416   */
417  @SuppressWarnings("ParameterName")
418  public void setF(double f) {
419    m_thisMutex.lock();
420    try {
421      m_F = f;
422    } finally {
423      m_thisMutex.unlock();
424    }
425  }
426
427  /**
428   * Get the Proportional coefficient.
429   *
430   * @return proportional coefficient
431   */
432  @Override
433  public double getP() {
434    m_thisMutex.lock();
435    try {
436      return m_P;
437    } finally {
438      m_thisMutex.unlock();
439    }
440  }
441
442  /**
443   * Get the Integral coefficient.
444   *
445   * @return integral coefficient
446   */
447  @Override
448  public double getI() {
449    m_thisMutex.lock();
450    try {
451      return m_I;
452    } finally {
453      m_thisMutex.unlock();
454    }
455  }
456
457  /**
458   * Get the Differential coefficient.
459   *
460   * @return differential coefficient
461   */
462  @Override
463  public double getD() {
464    m_thisMutex.lock();
465    try {
466      return m_D;
467    } finally {
468      m_thisMutex.unlock();
469    }
470  }
471
472  /**
473   * Get the Feed forward coefficient.
474   *
475   * @return feed forward coefficient
476   */
477  public double getF() {
478    m_thisMutex.lock();
479    try {
480      return m_F;
481    } finally {
482      m_thisMutex.unlock();
483    }
484  }
485
486  /**
487   * Return the current PID result This is always centered on zero and constrained the the max and
488   * min outs.
489   *
490   * @return the latest calculated output
491   */
492  public double get() {
493    m_thisMutex.lock();
494    try {
495      return m_result;
496    } finally {
497      m_thisMutex.unlock();
498    }
499  }
500
501  /**
502   * Set the PID controller to consider the input to be continuous, Rather then using the max and
503   * min input range as constraints, it considers them to be the same point and automatically
504   * calculates the shortest route to the setpoint.
505   *
506   * @param continuous Set to true turns on continuous, false turns off continuous
507   */
508  public void setContinuous(boolean continuous) {
509    if (continuous && m_inputRange <= 0) {
510      throw new IllegalStateException("No input range set when calling setContinuous().");
511    }
512    m_thisMutex.lock();
513    try {
514      m_continuous = continuous;
515    } finally {
516      m_thisMutex.unlock();
517    }
518  }
519
520  /**
521   * Set the PID controller to consider the input to be continuous, Rather then using the max and
522   * min input range as constraints, it considers them to be the same point and automatically
523   * calculates the shortest route to the setpoint.
524   */
525  public void setContinuous() {
526    setContinuous(true);
527  }
528
529  /**
530   * Sets the maximum and minimum values expected from the input and setpoint.
531   *
532   * @param minimumInput the minimum value expected from the input
533   * @param maximumInput the maximum value expected from the input
534   */
535  public void setInputRange(double minimumInput, double maximumInput) {
536    m_thisMutex.lock();
537    try {
538      if (minimumInput > maximumInput) {
539        throw new BoundaryException("Lower bound is greater than upper bound");
540      }
541      m_minimumInput = minimumInput;
542      m_maximumInput = maximumInput;
543      m_inputRange = maximumInput - minimumInput;
544    } finally {
545      m_thisMutex.unlock();
546    }
547
548    setSetpoint(m_setpoint);
549  }
550
551  /**
552   * Sets the minimum and maximum values to write.
553   *
554   * @param minimumOutput the minimum percentage to write to the output
555   * @param maximumOutput the maximum percentage to write to the output
556   */
557  public void setOutputRange(double minimumOutput, double maximumOutput) {
558    m_thisMutex.lock();
559    try {
560      if (minimumOutput > maximumOutput) {
561        throw new BoundaryException("Lower bound is greater than upper bound");
562      }
563      m_minimumOutput = minimumOutput;
564      m_maximumOutput = maximumOutput;
565    } finally {
566      m_thisMutex.unlock();
567    }
568  }
569
570  /**
571   * Set the setpoint for the PIDController.
572   *
573   * @param setpoint the desired setpoint
574   */
575  @Override
576  public void setSetpoint(double setpoint) {
577    m_thisMutex.lock();
578    try {
579      if (m_maximumInput > m_minimumInput) {
580        if (setpoint > m_maximumInput) {
581          m_setpoint = m_maximumInput;
582        } else if (setpoint < m_minimumInput) {
583          m_setpoint = m_minimumInput;
584        } else {
585          m_setpoint = setpoint;
586        }
587      } else {
588        m_setpoint = setpoint;
589      }
590    } finally {
591      m_thisMutex.unlock();
592    }
593  }
594
595  /**
596   * Returns the current setpoint of the PIDController.
597   *
598   * @return the current setpoint
599   */
600  @Override
601  public double getSetpoint() {
602    m_thisMutex.lock();
603    try {
604      return m_setpoint;
605    } finally {
606      m_thisMutex.unlock();
607    }
608  }
609
610  /**
611   * Returns the change in setpoint over time of the PIDController.
612   *
613   * @return the change in setpoint over time
614   */
615  public double getDeltaSetpoint() {
616    m_thisMutex.lock();
617    try {
618      return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
619    } finally {
620      m_thisMutex.unlock();
621    }
622  }
623
624  /**
625   * Returns the current difference of the input from the setpoint.
626   *
627   * @return the current error
628   */
629  @Override
630  public double getError() {
631    m_thisMutex.lock();
632    try {
633      return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet()));
634    } finally {
635      m_thisMutex.unlock();
636    }
637  }
638
639  /**
640   * Returns the current difference of the error over the past few iterations. You can specify the
641   * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
642   * used for the onTarget() function.
643   *
644   * @return the current average of the error
645   * @deprecated Use getError(), which is now already filtered.
646   */
647  @Deprecated
648  public double getAvgError() {
649    m_thisMutex.lock();
650    try {
651      return getError();
652    } finally {
653      m_thisMutex.unlock();
654    }
655  }
656
657  /**
658   * Sets what type of input the PID controller will use.
659   *
660   * @param pidSource the type of input
661   */
662  public void setPIDSourceType(PIDSourceType pidSource) {
663    m_pidInput.setPIDSourceType(pidSource);
664  }
665
666  /**
667   * Returns the type of input the PID controller is using.
668   *
669   * @return the PID controller input type
670   */
671  public PIDSourceType getPIDSourceType() {
672    return m_pidInput.getPIDSourceType();
673  }
674
675  /**
676   * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
677   * the range or as an absolute value. The Tolerance object encapsulates those options in an
678   * object. Use it by creating the type of tolerance that you want to use: setTolerance(new
679   * PIDController.AbsoluteTolerance(0.1))
680   *
681   * @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
682   *     AbsoluteTolerance
683   * @deprecated Use setPercentTolerance() instead.
684   */
685  @Deprecated
686  public void setTolerance(Tolerance tolerance) {
687    m_tolerance = tolerance;
688  }
689
690  /**
691   * Set the absolute error which is considered tolerable for use with OnTarget.
692   *
693   * @param absvalue absolute error which is tolerable in the units of the input object
694   */
695  public void setAbsoluteTolerance(double absvalue) {
696    m_thisMutex.lock();
697    try {
698      m_tolerance = new AbsoluteTolerance(absvalue);
699    } finally {
700      m_thisMutex.unlock();
701    }
702  }
703
704  /**
705   * Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
706   * 15 percent)
707   *
708   * @param percentage percent error which is tolerable
709   */
710  public void setPercentTolerance(double percentage) {
711    m_thisMutex.lock();
712    try {
713      m_tolerance = new PercentageTolerance(percentage);
714    } finally {
715      m_thisMutex.unlock();
716    }
717  }
718
719  /**
720   * Set the number of previous error samples to average for tolerancing. When determining whether a
721   * mechanism is on target, the user may want to use a rolling average of previous measurements
722   * instead of a precise position or velocity. This is useful for noisy sensors which return a few
723   * erroneous measurements when the mechanism is on target. However, the mechanism will not
724   * register as on target for at least the specified bufLength cycles.
725   *
726   * @param bufLength Number of previous cycles to average.
727   * @deprecated Use a LinearFilter as the input.
728   */
729  @Deprecated
730  public void setToleranceBuffer(int bufLength) {
731    m_thisMutex.lock();
732    try {
733      m_filter = LinearFilter.movingAverage(bufLength);
734    } finally {
735      m_thisMutex.unlock();
736    }
737  }
738
739  /**
740   * Return true if the error is within the percentage of the total input range, determined by
741   * setTolerance. This assumes that the maximum and minimum input were set using setInput.
742   *
743   * @return true if the error is less than the tolerance
744   */
745  public boolean onTarget() {
746    m_thisMutex.lock();
747    try {
748      return m_tolerance.onTarget();
749    } finally {
750      m_thisMutex.unlock();
751    }
752  }
753
754  /** Reset the previous error, the integral term, and disable the controller. */
755  @Override
756  public void reset() {
757    m_thisMutex.lock();
758    try {
759      m_prevError = 0;
760      m_totalError = 0;
761      m_result = 0;
762    } finally {
763      m_thisMutex.unlock();
764    }
765  }
766
767  /**
768   * Passes the output directly to setSetpoint().
769   *
770   * <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
771   * In that case, the output of the parent controller becomes the input (i.e., the reference) of
772   * the child.
773   *
774   * <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
775   */
776  @Override
777  public void pidWrite(double output) {
778    setSetpoint(output);
779  }
780
781  @Override
782  public void initSendable(SendableBuilder builder) {
783    builder.setSmartDashboardType("PIDController");
784    builder.setSafeState(this::reset);
785    builder.addDoubleProperty("p", this::getP, this::setP);
786    builder.addDoubleProperty("i", this::getI, this::setI);
787    builder.addDoubleProperty("d", this::getD, this::setD);
788    builder.addDoubleProperty("f", this::getF, this::setF);
789    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
790  }
791
792  /**
793   * Wraps error around for continuous inputs. The original error is returned if continuous mode is
794   * disabled. This is an unsynchronized function.
795   *
796   * @param error The current error of the PID controller.
797   * @return Error for continuous inputs.
798   */
799  protected double getContinuousError(double error) {
800    if (m_continuous && m_inputRange > 0) {
801      error %= m_inputRange;
802      if (Math.abs(error) > m_inputRange / 2) {
803        if (error > 0) {
804          return error - m_inputRange;
805        } else {
806          return error + m_inputRange;
807        }
808      }
809    }
810
811    return error;
812  }
813
814  private static double clamp(double value, double low, double high) {
815    return Math.max(low, Math.min(value, high));
816  }
817}