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 edu.wpi.first.util.sendable.SendableBuilder;
008
009/**
010 * Class implements a PID Control Loop.
011 *
012 * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
013 * calculations, as well as writing the given PIDOutput.
014 *
015 * <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
016 * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
017 * given set of PID constants.
018 *
019 * @deprecated Use {@link edu.wpi.first.math.controller.PIDController} instead.
020 */
021@Deprecated(since = "2020", forRemoval = true)
022public class PIDController extends PIDBase implements Controller {
023  Notifier m_controlLoop = new Notifier(this::calculate);
024
025  /**
026   * Allocate a PID object with the given constants for P, I, D, and F.
027   *
028   * @param Kp the proportional coefficient
029   * @param Ki the integral coefficient
030   * @param Kd the derivative coefficient
031   * @param Kf the feed forward term
032   * @param source The PIDSource object that is used to get values
033   * @param output The PIDOutput object that is set to the output percentage
034   * @param period the loop time for doing calculations in seconds. This particularly affects
035   *     calculations of the integral and differential terms. The default is 0.05 (50ms).
036   */
037  @SuppressWarnings("ParameterName")
038  public PIDController(
039      double Kp,
040      double Ki,
041      double Kd,
042      double Kf,
043      PIDSource source,
044      PIDOutput output,
045      double period) {
046    super(Kp, Ki, Kd, Kf, source, output);
047    m_controlLoop.startPeriodic(period);
048  }
049
050  /**
051   * Allocate a PID object with the given constants for P, I, D and period.
052   *
053   * @param Kp the proportional coefficient
054   * @param Ki the integral coefficient
055   * @param Kd the derivative coefficient
056   * @param source the PIDSource object that is used to get values
057   * @param output the PIDOutput object that is set to the output percentage
058   * @param period the loop time for doing calculations in seconds. This particularly affects
059   *     calculations of the integral and differential terms. The default is 0.05 (50ms).
060   */
061  @SuppressWarnings("ParameterName")
062  public PIDController(
063      double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
064    this(Kp, Ki, Kd, 0.0, source, output, period);
065  }
066
067  /**
068   * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
069   *
070   * @param Kp the proportional coefficient
071   * @param Ki the integral coefficient
072   * @param Kd the derivative coefficient
073   * @param source The PIDSource object that is used to get values
074   * @param output The PIDOutput object that is set to the output percentage
075   */
076  @SuppressWarnings("ParameterName")
077  public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
078    this(Kp, Ki, Kd, source, output, kDefaultPeriod);
079  }
080
081  /**
082   * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
083   *
084   * @param Kp the proportional coefficient
085   * @param Ki the integral coefficient
086   * @param Kd the derivative coefficient
087   * @param Kf the feed forward term
088   * @param source The PIDSource object that is used to get values
089   * @param output The PIDOutput object that is set to the output percentage
090   */
091  @SuppressWarnings("ParameterName")
092  public PIDController(
093      double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
094    this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
095  }
096
097  @Override
098  public void close() {
099    m_controlLoop.close();
100    m_thisMutex.lock();
101    try {
102      m_pidOutput = null;
103      m_pidInput = null;
104      m_controlLoop = null;
105    } finally {
106      m_thisMutex.unlock();
107    }
108  }
109
110  /** Begin running the PIDController. */
111  @Override
112  public void enable() {
113    m_thisMutex.lock();
114    try {
115      m_enabled = true;
116    } finally {
117      m_thisMutex.unlock();
118    }
119  }
120
121  /** Stop running the PIDController, this sets the output to zero before stopping. */
122  @Override
123  public void disable() {
124    // Ensures m_enabled check and pidWrite() call occur atomically
125    m_pidWriteMutex.lock();
126    try {
127      m_thisMutex.lock();
128      try {
129        m_enabled = false;
130      } finally {
131        m_thisMutex.unlock();
132      }
133
134      m_pidOutput.pidWrite(0);
135    } finally {
136      m_pidWriteMutex.unlock();
137    }
138  }
139
140  /**
141   * Set the enabled state of the PIDController.
142   *
143   * @param enable True to enable the PIDController.
144   */
145  public void setEnabled(boolean enable) {
146    if (enable) {
147      enable();
148    } else {
149      disable();
150    }
151  }
152
153  /**
154   * Return true if PIDController is enabled.
155   *
156   * @return True if PIDController is enabled.
157   */
158  public boolean isEnabled() {
159    m_thisMutex.lock();
160    try {
161      return m_enabled;
162    } finally {
163      m_thisMutex.unlock();
164    }
165  }
166
167  /** Reset the previous error, the integral term, and disable the controller. */
168  @Override
169  public void reset() {
170    disable();
171
172    super.reset();
173  }
174
175  @Override
176  public void initSendable(SendableBuilder builder) {
177    super.initSendable(builder);
178    builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
179  }
180}