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 package edu.wpi.first.wpilibj;
008
009 import edu.wpi.first.wpilibj.communication.UsageReporting;
010 import edu.wpi.first.wpilibj.parsing.IUtility;
011 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
012 import edu.wpi.first.wpilibj.tables.ITable;
013 import edu.wpi.first.wpilibj.tables.ITableListener;
014 import edu.wpi.first.wpilibj.util.BoundaryException;
015 import java.util.TimerTask;
016
017 /**
018 * Class implements a PID Control Loop.
019 *
020 * Creates a separate thread which reads the given PIDSource and takes
021 * care of the integral calculations, as well as writing the given
022 * PIDOutput
023 */
024 public class PIDController implements IUtility, LiveWindowSendable, Controller {
025
026 public static final double kDefaultPeriod = .05;
027 private static int instances = 0;
028 private double m_P; // factor for "proportional" control
029 private double m_I; // factor for "integral" control
030 private double m_D; // factor for "derivative" control
031 private double m_F; // factor for feedforward term
032 private double m_maximumOutput = 1.0; // |maximum output|
033 private double m_minimumOutput = -1.0; // |minimum output|
034 private double m_maximumInput = 0.0; // maximum input - limit setpoint to this
035 private double m_minimumInput = 0.0; // minimum input - limit setpoint to this
036 private boolean m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder
037 private boolean m_enabled = false; //is the pid controller enabled
038 private double m_prevError = 0.0; // the prior sensor input (used to compute velocity)
039 private double m_totalError = 0.0; //the sum of the errors for use in the integral calc
040 private Tolerance m_tolerance; //the tolerance object used to check if on target
041 private double m_setpoint = 0.0;
042 private double m_error = 0.0;
043 private double m_result = 0.0;
044 private double m_period = kDefaultPeriod;
045 PIDSource m_pidInput;
046 PIDOutput m_pidOutput;
047 java.util.Timer m_controlLoop;
048 private boolean m_usingPercentTolerance;
049
050 /**
051 * Tolerance is the type of tolerance used to specify if the PID controller is on target.
052 * The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
053 * specify types of tolerance specifications to use.
054 */
055 public interface Tolerance {
056 public boolean onTarget();
057 }
058
059 public class PercentageTolerance implements Tolerance {
060 double percentage;
061
062 PercentageTolerance(double value) {
063 percentage = value;
064 }
065
066 public boolean onTarget() {
067 return (Math.abs(getError()) < percentage / 100
068 * (m_maximumInput - m_minimumInput));
069 }
070 }
071
072 public class AbsoluteTolerance implements Tolerance {
073 double value;
074
075 AbsoluteTolerance(double value) {
076 this.value = value;
077 }
078
079 public boolean onTarget() {
080 return Math.abs(getError()) < value;
081 }
082 }
083
084 public class NullTolerance implements Tolerance {
085
086 public boolean onTarget() {
087 throw new RuntimeException("No tolerance value set when using PIDController.onTarget()");
088 }
089 }
090
091 private class PIDTask extends TimerTask {
092
093 private PIDController m_controller;
094
095 public PIDTask(PIDController controller) {
096 if (controller == null) {
097 throw new NullPointerException("Given PIDController was null");
098 }
099 m_controller = controller;
100 }
101
102 public void run() {
103 m_controller.calculate();
104 }
105 }
106
107 /**
108 * Allocate a PID object with the given constants for P, I, D, and F
109 * @param Kp the proportional coefficient
110 * @param Ki the integral coefficient
111 * @param Kd the derivative coefficient
112 * @param Kf the feed forward term
113 * @param source The PIDSource object that is used to get values
114 * @param output The PIDOutput object that is set to the output percentage
115 * @param period the loop time for doing calculations. This particularly effects calculations of the
116 * integral and differential terms. The default is 50ms.
117 */
118 public PIDController(double Kp, double Ki, double Kd, double Kf,
119 PIDSource source, PIDOutput output,
120 double period) {
121
122 if (source == null) {
123 throw new NullPointerException("Null PIDSource was given");
124 }
125 if (output == null) {
126 throw new NullPointerException("Null PIDOutput was given");
127 }
128
129 m_controlLoop = new java.util.Timer();
130
131
132 m_P = Kp;
133 m_I = Ki;
134 m_D = Kd;
135 m_F = Kf;
136
137 m_pidInput = source;
138 m_pidOutput = output;
139 m_period = period;
140
141 m_controlLoop.schedule(new PIDTask(this), 0L, (long) (m_period * 1000));
142
143 instances++;
144 UsageReporting.report(UsageReporting.kResourceType_PIDController, instances);
145 m_tolerance = new NullTolerance();
146 }
147
148 /**
149 * Allocate a PID object with the given constants for P, I, D and period
150 * @param Kp
151 * @param Ki
152 * @param Kd
153 * @param source
154 * @param output
155 * @param period
156 */
157 public PIDController(double Kp, double Ki, double Kd,
158 PIDSource source, PIDOutput output,
159 double period) {
160 this(Kp, Ki, Kd, 0.0, source, output, period);
161 }
162
163 /**
164 * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
165 * @param Kp the proportional coefficient
166 * @param Ki the integral coefficient
167 * @param Kd the derivative coefficient
168 * @param source The PIDSource object that is used to get values
169 * @param output The PIDOutput object that is set to the output percentage
170 */
171 public PIDController(double Kp, double Ki, double Kd,
172 PIDSource source, PIDOutput output) {
173 this(Kp, Ki, Kd, source, output, kDefaultPeriod);
174 }
175
176 /**
177 * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
178 * @param Kp the proportional coefficient
179 * @param Ki the integral coefficient
180 * @param Kd the derivative coefficient
181 * @param source The PIDSource object that is used to get values
182 * @param output The PIDOutput object that is set to the output percentage
183 */
184 public PIDController(double Kp, double Ki, double Kd, double Kf,
185 PIDSource source, PIDOutput output) {
186 this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
187 }
188
189 /**
190 * Free the PID object
191 */
192 public void free() {
193 m_controlLoop.cancel();
194 m_controlLoop = null;
195 }
196
197 /**
198 * Read the input, calculate the output accordingly, and write to the output.
199 * This should only be called by the PIDTask
200 * and is created during initialization.
201 */
202 protected void calculate() {
203 boolean enabled;
204 PIDSource pidInput;
205
206 synchronized (this) {
207 if (m_pidInput == null) {
208 return;
209 }
210 if (m_pidOutput == null) {
211 return;
212 }
213 enabled = m_enabled; // take snapshot of these values...
214 pidInput = m_pidInput;
215 }
216
217 if (enabled) {
218 double input = pidInput.pidGet();
219 double result;
220 PIDOutput pidOutput = null;
221
222 synchronized (this) {
223 m_error = m_setpoint - input;
224 if (m_continuous) {
225 if (Math.abs(m_error)
226 > (m_maximumInput - m_minimumInput) / 2) {
227 if (m_error > 0) {
228 m_error = m_error - m_maximumInput + m_minimumInput;
229 } else {
230 m_error = m_error
231 + m_maximumInput - m_minimumInput;
232 }
233 }
234 }
235
236 if (m_I != 0)
237 {
238 double potentialIGain = (m_totalError + m_error) * m_I;
239 if (potentialIGain < m_maximumOutput)
240 {
241 if (potentialIGain > m_minimumOutput) {
242 m_totalError += m_error;
243 }
244 else {
245 m_totalError = m_minimumOutput / m_I;
246 }
247 }
248 else
249 {
250 m_totalError = m_maximumOutput / m_I;
251 }
252 }
253
254 m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F;
255 m_prevError = m_error;
256
257 if (m_result > m_maximumOutput) {
258 m_result = m_maximumOutput;
259 } else if (m_result < m_minimumOutput) {
260 m_result = m_minimumOutput;
261 }
262 pidOutput = m_pidOutput;
263 result = m_result;
264 }
265
266 pidOutput.pidWrite(result);
267 }
268 }
269
270 /**
271 * Set the PID Controller gain parameters.
272 * Set the proportional, integral, and differential coefficients.
273 * @param p Proportional coefficient
274 * @param i Integral coefficient
275 * @param d Differential coefficient
276 */
277 public synchronized void setPID(double p, double i, double d) {
278 m_P = p;
279 m_I = i;
280 m_D = d;
281
282 if (table != null) {
283 table.putNumber("p", p);
284 table.putNumber("i", i);
285 table.putNumber("d", d);
286 }
287 }
288
289 /**
290 * Set the PID Controller gain parameters.
291 * Set the proportional, integral, and differential coefficients.
292 * @param p Proportional coefficient
293 * @param i Integral coefficient
294 * @param d Differential coefficient
295 * @param f Feed forward coefficient
296 */
297 public synchronized void setPID(double p, double i, double d, double f) {
298 m_P = p;
299 m_I = i;
300 m_D = d;
301 m_F = f;
302
303 if (table != null) {
304 table.putNumber("p", p);
305 table.putNumber("i", i);
306 table.putNumber("d", d);
307 table.putNumber("f", f);
308 }
309 }
310
311 /**
312 * Get the Proportional coefficient
313 * @return proportional coefficient
314 */
315 public double getP() {
316 return m_P;
317 }
318
319 /**
320 * Get the Integral coefficient
321 * @return integral coefficient
322 */
323 public double getI() {
324 return m_I;
325 }
326
327 /**
328 * Get the Differential coefficient
329 * @return differential coefficient
330 */
331 public synchronized double getD() {
332 return m_D;
333 }
334
335 /**
336 * Get the Feed forward coefficient
337 * @return feed forward coefficient
338 */
339 public synchronized double getF() {
340 return m_F;
341 }
342
343 /**
344 * Return the current PID result
345 * This is always centered on zero and constrained the the max and min outs
346 * @return the latest calculated output
347 */
348 public synchronized double get() {
349 return m_result;
350 }
351
352 /**
353 * Set the PID controller to consider the input to be continuous,
354 * Rather then using the max and min in as constraints, it considers them to
355 * be the same point and automatically calculates the shortest route to
356 * the setpoint.
357 * @param continuous Set to true turns on continuous, false turns off continuous
358 */
359 public synchronized void setContinuous(boolean continuous) {
360 m_continuous = continuous;
361 }
362
363 /**
364 * Set the PID controller to consider the input to be continuous,
365 * Rather then using the max and min in as constraints, it considers them to
366 * be the same point and automatically calculates the shortest route to
367 * the setpoint.
368 */
369 public synchronized void setContinuous() {
370 this.setContinuous(true);
371 }
372
373 /**
374 * Sets the maximum and minimum values expected from the input.
375 *
376 * @param minimumInput the minimum percentage expected from the input
377 * @param maximumInput the maximum percentage expected from the output
378 */
379 public synchronized void setInputRange(double minimumInput, double maximumInput) {
380 if (minimumInput > maximumInput) {
381 throw new BoundaryException("Lower bound is greater than upper bound");
382 }
383 m_minimumInput = minimumInput;
384 m_maximumInput = maximumInput;
385 setSetpoint(m_setpoint);
386 }
387
388 /**
389 * Sets the minimum and maximum values to write.
390 *
391 * @param minimumOutput the minimum percentage to write to the output
392 * @param maximumOutput the maximum percentage to write to the output
393 */
394 public synchronized void setOutputRange(double minimumOutput, double maximumOutput) {
395 if (minimumOutput > maximumOutput) {
396 throw new BoundaryException("Lower bound is greater than upper bound");
397 }
398 m_minimumOutput = minimumOutput;
399 m_maximumOutput = maximumOutput;
400 }
401
402 /**
403 * Set the setpoint for the PIDController
404 * @param setpoint the desired setpoint
405 */
406 public synchronized void setSetpoint(double setpoint) {
407 if (m_maximumInput > m_minimumInput) {
408 if (setpoint > m_maximumInput) {
409 m_setpoint = m_maximumInput;
410 } else if (setpoint < m_minimumInput) {
411 m_setpoint = m_minimumInput;
412 } else {
413 m_setpoint = setpoint;
414 }
415 } else {
416 m_setpoint = setpoint;
417 }
418
419 if (table != null)
420 table.putNumber("setpoint", m_setpoint);
421 }
422
423 /**
424 * Returns the current setpoint of the PIDController
425 * @return the current setpoint
426 */
427 public synchronized double getSetpoint() {
428 return m_setpoint;
429 }
430
431 /**
432 * Returns the current difference of the input from the setpoint
433 * @return the current error
434 */
435 public synchronized double getError() {
436 //return m_error;
437 return getSetpoint() - m_pidInput.pidGet();
438 }
439
440 /**
441 * Set the percentage error which is considered tolerable for use with
442 * OnTarget. (Input of 15.0 = 15 percent)
443 * @param percent error which is tolerable
444 * @deprecated Use setTolerance(Tolerance), i.e. setTolerance(new PIDController.PercentageTolerance(15))
445 */
446 public synchronized void setTolerance(double percent) {
447 m_tolerance = new PercentageTolerance(percent);
448 }
449
450 /** Set the PID tolerance using a Tolerance object.
451 * Tolerance can be specified as a percentage of the range or as an absolute
452 * value. The Tolerance object encapsulates those options in an object. Use it by
453 * creating the type of tolerance that you want to use: setTolerance(new PIDController.AbsoluteTolerance(0.1))
454 * @param tolerance a tolerance object of the right type, e.g. PercentTolerance
455 * or AbsoluteTolerance
456 */
457 private void setTolerance(Tolerance tolerance) {
458 m_tolerance = tolerance;
459 }
460
461 /**
462 * Set the absolute error which is considered tolerable for use with
463 * OnTarget.
464 * @param absolute error which is tolerable in the units of the input object
465 */
466 public synchronized void setAbsoluteTolerance(double absvalue) {
467 m_tolerance = new AbsoluteTolerance(absvalue);
468 }
469
470 /**
471 * Set the percentage error which is considered tolerable for use with
472 * OnTarget. (Input of 15.0 = 15 percent)
473 * @param percent error which is tolerable
474 */
475 public synchronized void setPercentTolerance(double percentage) {
476 m_tolerance = new PercentageTolerance(percentage);
477 }
478
479 /**
480 * Return true if the error is within the percentage of the total input range,
481 * determined by setTolerance. This assumes that the maximum and minimum input
482 * were set using setInput.
483 * @return true if the error is less than the tolerance
484 */
485 public synchronized boolean onTarget() {
486 return m_tolerance.onTarget();
487 }
488
489 /**
490 * Begin running the PIDController
491 */
492 public synchronized void enable() {
493 m_enabled = true;
494
495 if (table != null) {
496 table.putBoolean("enabled", true);
497 }
498 }
499
500 /**
501 * Stop running the PIDController, this sets the output to zero before stopping.
502 */
503 public synchronized void disable() {
504 m_pidOutput.pidWrite(0);
505 m_enabled = false;
506
507 if (table != null) {
508 table.putBoolean("enabled", false);
509 }
510 }
511
512 /**
513 * Return true if PIDController is enabled.
514 */
515 public synchronized boolean isEnable() {
516 return m_enabled;
517 }
518
519 /**
520 * Reset the previous error,, the integral term, and disable the controller.
521 */
522 public synchronized void reset() {
523 disable();
524 m_prevError = 0;
525 m_totalError = 0;
526 m_result = 0;
527 }
528
529 public String getSmartDashboardType(){
530 return "PIDController";
531 }
532
533 private ITableListener listener = new ITableListener() {
534 public void valueChanged(ITable table, String key, Object value, boolean isNew) {
535 if (key.equals("p") || key.equals("i") || key.equals("d") || key.equals("f")) {
536 if (m_P != table.getNumber("p", 0.0) || m_I != table.getNumber("i", 0.0) ||
537 m_D != table.getNumber("d", 0.0) || m_F != table.getNumber("f", 0.0))
538 setPID(table.getNumber("p", 0.0), table.getNumber("i", 0.0), table.getNumber("d", 0.0), table.getNumber("f", 0.0));
539 } else if (key.equals("setpoint")) {
540 if (m_setpoint != ((Double) value).doubleValue())
541 setSetpoint(((Double) value).doubleValue());
542 } else if (key.equals("enabled")) {
543 if (m_enabled != ((Boolean) value).booleanValue()) {
544 if (((Boolean) value).booleanValue()) {
545 enable();
546 } else {
547 disable();
548 }
549 }
550 }
551 }
552 };
553 private ITable table;
554 public void initTable(ITable table){
555 if(this.table!=null)
556 this.table.removeTableListener(listener);
557 this.table = table;
558 if(table!=null){
559 table.putNumber("p", getP());
560 table.putNumber("i", getI());
561 table.putNumber("d", getD());
562 table.putNumber("f", getF());
563 table.putNumber("setpoint", getSetpoint());
564 table.putBoolean("enabled", isEnable());
565 table.addTableListener(listener, false);
566 }
567 }
568
569 /**
570 * {@inheritDoc}
571 */
572 public ITable getTable(){
573 return table;
574 }
575
576 /**
577 * {@inheritDoc}
578 */
579 public void updateTable() {
580 }
581
582 /**
583 * {@inheritDoc}
584 */
585 public void startLiveWindowMode() {
586 disable();
587 }
588
589 /**
590 * {@inheritDoc}
591 */
592 public void stopLiveWindowMode() {
593 }
594 }