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.command;
006
007import edu.wpi.first.wpilibj.PIDController;
008import edu.wpi.first.wpilibj.PIDOutput;
009import edu.wpi.first.wpilibj.PIDSource;
010import edu.wpi.first.wpilibj.PIDSourceType;
011
012/**
013 * This class is designed to handle the case where there is a {@link Subsystem} which uses a single
014 * {@link PIDController} almost constantly (for instance, an elevator which attempts to stay at a
015 * constant height).
016 *
017 * <p>It provides some convenience methods to run an internal {@link PIDController} . It also allows
018 * access to the internal {@link PIDController} in order to give total control to the programmer.
019 */
020public abstract class PIDSubsystem extends Subsystem {
021  /** The internal {@link PIDController}. */
022  private final PIDController m_controller;
023  /** An output which calls {@link PIDCommand#usePIDOutput(double)}. */
024  private final PIDOutput m_output = this::usePIDOutput;
025
026  /** A source which calls {@link PIDCommand#returnPIDInput()}. */
027  private final PIDSource m_source =
028      new PIDSource() {
029        @Override
030        public void setPIDSourceType(PIDSourceType pidSource) {}
031
032        @Override
033        public PIDSourceType getPIDSourceType() {
034          return PIDSourceType.kDisplacement;
035        }
036
037        @Override
038        public double pidGet() {
039          return returnPIDInput();
040        }
041      };
042
043  /**
044   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
045   *
046   * @param name the name
047   * @param p the proportional value
048   * @param i the integral value
049   * @param d the derivative value
050   */
051  public PIDSubsystem(String name, double p, double i, double d) {
052    super(name);
053    m_controller = new PIDController(p, i, d, m_source, m_output);
054    addChild("PIDController", m_controller);
055  }
056
057  /**
058   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values.
059   *
060   * @param name the name
061   * @param p the proportional value
062   * @param i the integral value
063   * @param d the derivative value
064   * @param f the feed forward value
065   */
066  public PIDSubsystem(String name, double p, double i, double d, double f) {
067    super(name);
068    m_controller = new PIDController(p, i, d, f, m_source, m_output);
069    addChild("PIDController", m_controller);
070  }
071
072  /**
073   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will also
074   * space the time between PID loop calculations to be equal to the given period.
075   *
076   * @param name the name
077   * @param p the proportional value
078   * @param i the integral value
079   * @param d the derivative value
080   * @param f the feed forward value
081   * @param period the time (in seconds) between calculations
082   */
083  public PIDSubsystem(String name, double p, double i, double d, double f, double period) {
084    super(name);
085    m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
086    addChild("PIDController", m_controller);
087  }
088
089  /**
090   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
091   * class name as its name.
092   *
093   * @param p the proportional value
094   * @param i the integral value
095   * @param d the derivative value
096   */
097  public PIDSubsystem(double p, double i, double d) {
098    m_controller = new PIDController(p, i, d, m_source, m_output);
099    addChild("PIDController", m_controller);
100  }
101
102  /**
103   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will use
104   * the class name as its name. It will also space the time between PID loop calculations to be
105   * equal to the given period.
106   *
107   * @param p the proportional value
108   * @param i the integral value
109   * @param d the derivative value
110   * @param f the feed forward coefficient
111   * @param period the time (in seconds) between calculations
112   */
113  public PIDSubsystem(double p, double i, double d, double f, double period) {
114    m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
115    addChild("PIDController", m_controller);
116  }
117
118  /**
119   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
120   * class name as its name. It will also space the time between PID loop calculations to be equal
121   * to the given period.
122   *
123   * @param p the proportional value
124   * @param i the integral value
125   * @param d the derivative value
126   * @param period the time (in seconds) between calculations
127   */
128  public PIDSubsystem(double p, double i, double d, double period) {
129    m_controller = new PIDController(p, i, d, m_source, m_output, period);
130    addChild("PIDController", m_controller);
131  }
132
133  /**
134   * Returns the {@link PIDController} used by this {@link PIDSubsystem}. Use this if you would like
135   * to fine tune the pid loop.
136   *
137   * @return the {@link PIDController} used by this {@link PIDSubsystem}
138   */
139  public PIDController getPIDController() {
140    return m_controller;
141  }
142
143  /**
144   * Adds the given value to the setpoint. If {@link PIDSubsystem#setInputRange(double, double)
145   * setInputRange(...)} was used, then the bounds will still be honored by this method.
146   *
147   * @param deltaSetpoint the change in the setpoint
148   */
149  public void setSetpointRelative(double deltaSetpoint) {
150    setSetpoint(getPosition() + deltaSetpoint);
151  }
152
153  /**
154   * Sets the setpoint to the given value. If {@link PIDSubsystem#setInputRange(double, double)
155   * setInputRange(...)} was called, then the given setpoint will be trimmed to fit within the
156   * range.
157   *
158   * @param setpoint the new setpoint
159   */
160  public void setSetpoint(double setpoint) {
161    m_controller.setSetpoint(setpoint);
162  }
163
164  /**
165   * Returns the setpoint.
166   *
167   * @return the setpoint
168   */
169  public double getSetpoint() {
170    return m_controller.getSetpoint();
171  }
172
173  /**
174   * Returns the current position.
175   *
176   * @return the current position
177   */
178  public double getPosition() {
179    return returnPIDInput();
180  }
181
182  /**
183   * Sets the maximum and minimum values expected from the input.
184   *
185   * @param minimumInput the minimum value expected from the input
186   * @param maximumInput the maximum value expected from the output
187   */
188  public void setInputRange(double minimumInput, double maximumInput) {
189    m_controller.setInputRange(minimumInput, maximumInput);
190  }
191
192  /**
193   * Sets the maximum and minimum values to write.
194   *
195   * @param minimumOutput the minimum value to write to the output
196   * @param maximumOutput the maximum value to write to the output
197   */
198  public void setOutputRange(double minimumOutput, double maximumOutput) {
199    m_controller.setOutputRange(minimumOutput, maximumOutput);
200  }
201
202  /**
203   * Set the absolute error which is considered tolerable for use with OnTarget. The value is in the
204   * same range as the PIDInput values.
205   *
206   * @param t the absolute tolerance
207   */
208  public void setAbsoluteTolerance(double t) {
209    m_controller.setAbsoluteTolerance(t);
210  }
211
212  /**
213   * Set the percentage error which is considered tolerable for use with OnTarget. (Value of 15.0 ==
214   * 15 percent).
215   *
216   * @param p the percent tolerance
217   */
218  public void setPercentTolerance(double p) {
219    m_controller.setPercentTolerance(p);
220  }
221
222  /**
223   * Return true if the error is within the percentage of the total input range, determined by
224   * setTolerance. This assumes that the maximum and minimum input were set using setInput.
225   *
226   * @return true if the error is less than the tolerance
227   */
228  public boolean onTarget() {
229    return m_controller.onTarget();
230  }
231
232  /**
233   * Returns the input for the pid loop.
234   *
235   * <p>It returns the input for the pid loop, so if this Subsystem was based off of a gyro, then it
236   * should return the angle of the gyro.
237   *
238   * <p>All subclasses of {@link PIDSubsystem} must override this method.
239   *
240   * @return the value the pid loop should use as input
241   */
242  protected abstract double returnPIDInput();
243
244  /**
245   * Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
246   * This method is a good time to set motor values, maybe something along the lines of <code>
247   * driveline.tankDrive(output, -output)</code>.
248   *
249   * <p>All subclasses of {@link PIDSubsystem} must override this method.
250   *
251   * @param output the value the pid loop calculated
252   */
253  protected abstract void usePIDOutput(double output);
254
255  /** Enables the internal {@link PIDController}. */
256  public void enable() {
257    m_controller.enable();
258  }
259
260  /** Disables the internal {@link PIDController}. */
261  public void disable() {
262    m_controller.disable();
263  }
264}