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