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