Bryce Williams / PID Featured

Basic but robust PID library

Dependents:   ESP8266_pid_mtrPos_webserver_SDcard_v2 ESP8266_pid_mtrSpeed_Webserver_SDcard ESP8266_pid_spd_and_pos_webserver_SDcard ESP8266_pid_redbot_webserver ... more

PID.cpp@5:0cf2f6d13c71, 2015-12-29 (annotated)

Committer:
electromotivated
Date:
Tue Dec 29 02:31:32 2015 +0000
Revision:
5:0cf2f6d13c71
Parent:
4:4ed1f5bccac8 
Added Comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
electromotivated 0:9a6f7aafe531 1 #include "PID.h"
electromotivated 0:9a6f7aafe531 2 /*
electromotivated 0:9a6f7aafe531 3 Bryce Williams 11/19/2015
electromotivated 0:9a6f7aafe531 4 See PID.h for references as well as method descriptions
electromotivated 0:9a6f7aafe531 5 */
electromotivated 0:9a6f7aafe531 6
electromotivated 0:9a6f7aafe531 7 PID::PID(float* setpoint, float* feedback, float* output,
electromotivated 0:9a6f7aafe531 8 float output_lower, float output_upper,
electromotivated 0:9a6f7aafe531 9 float kp, float ki, float kd, float Ts){
electromotivated 0:9a6f7aafe531 10 _Ts = Ts; // Init params
electromotivated 0:9a6f7aafe531 11 _kp = kp;
electromotivated 0:9a6f7aafe531 12 _ki = ki*Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 13 _kd = kd / Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 14
electromotivated 0:9a6f7aafe531 15 _setpoint = setpoint;
electromotivated 0:9a6f7aafe531 16 _feedback = feedback;
electromotivated 0:9a6f7aafe531 17 _output = output;
electromotivated 0:9a6f7aafe531 18
electromotivated 0:9a6f7aafe531 19 _output_lower = output_lower;
electromotivated 0:9a6f7aafe531 20 _output_upper = output_upper;
electromotivated 0:9a6f7aafe531 21 }
electromotivated 0:9a6f7aafe531 22
electromotivated 0:9a6f7aafe531 23 void PID::start(){
electromotivated 0:9a6f7aafe531 24 // Start up such we avoid bumps... (see "Initialization" section in
electromotivated 0:9a6f7aafe531 25 // the reference link found in the header file).
electromotivated 3:6c2c985408df 26 last_feedback = *_feedback; // Eliminate derivative kick at start/restart
electromotivated 0:9a6f7aafe531 27 i_accumulator = clip(*_output, _output_lower,
electromotivated 0:9a6f7aafe531 28 _output_upper); // P and D terms are zero, thus
electromotivated 0:9a6f7aafe531 29 // i term is used to keep output unchanged
electromotivated 3:6c2c985408df 30 /*
electromotivated 3:6c2c985408df 31 If Ki is set to zero we must "flush" the intergral accumulator.
electromotivated 3:6c2c985408df 32
electromotivated 3:6c2c985408df 33 Reason:
electromotivated 3:6c2c985408df 34 If we don't "flush", i_accumulator will hold the output value from line
electromotivated 3:6c2c985408df 35 above, and because Ki is now zero only zeros will be added to
electromotivated 3:6c2c985408df 36 i_accumulator in the sample method, and thus i_accumulator is left
electromotivated 3:6c2c985408df 37 unchanged from here on out. i_accumulator is now a constant of value
electromotivated 3:6c2c985408df 38 output from line above and will ALWAYS appear in the output. i.e.
electromotivated 3:6c2c985408df 39
electromotivated 3:6c2c985408df 40 Here is the BUG if we DON'T FLUSH
electromotivated 3:6c2c985408df 41
electromotivated 3:6c2c985408df 42 _ki = 0; // User set ki = zero using PID::set_parameters()
electromotivated 3:6c2c985408df 43
electromotivated 3:6c2c985408df 44 THEN when PID::set_parameters() calls PID::start() (this method)
electromotivated 3:6c2c985408df 45
electromotivated 3:6c2c985408df 46 i_accumulator = output; // From line above
electromotivated 3:6c2c985408df 47
electromotivated 3:6c2c985408df 48 Then when PID::sample() is called everytime...
electromotivated 3:6c2c985408df 49
electromotivated 3:6c2c985408df 50 sample(){
electromotivated 3:6c2c985408df 51 i_accumulator += _ki * error; // Now this is equivalent to
electromotivated 3:6c2c985408df 52 // i_accumulator = output + 0
electromotivated 3:6c2c985408df 53 // which always equals output
electromotivated 3:6c2c985408df 54 // value from line above
electromotivated 3:6c2c985408df 55
electromotivated 3:6c2c985408df 56 i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
electromotivated 3:6c2c985408df 57
electromotivated 3:6c2c985408df 58 // Run it!
electromotivated 3:6c2c985408df 59 *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
electromotivated 3:6c2c985408df 60 // i_accumulator is fixed at value output from line above
electromotivated 3:6c2c985408df 61 // i.e. i_accumulator = clip(*_output, _output_lower,
electromotivated 3:6c2c985408df 62 _output_upper) = output;
electromotivated 3:6c2c985408df 63 last_feedback = *_feedback;
electromotivated 3:6c2c985408df 64 // Clamp Output
electromotivated 3:6c2c985408df 65 *_output = clip(*_output, _output_lower, _output_upper);
electromotivated 3:6c2c985408df 66 // Here *_output will always be offset by "output" value
electromotivated 3:6c2c985408df 67 // from line above
electromotivated 3:6c2c985408df 68 }
electromotivated 3:6c2c985408df 69 */
electromotivated 3:6c2c985408df 70 if(-0.00001 <= _ki && _ki <= 0.00001) i_accumulator = 0;
electromotivated 0:9a6f7aafe531 71 sample_timer.attach(this, &PID::sample, _Ts);
electromotivated 0:9a6f7aafe531 72 }
electromotivated 0:9a6f7aafe531 73
electromotivated 0:9a6f7aafe531 74 void PID::stop(){
electromotivated 0:9a6f7aafe531 75 sample_timer.detach();
electromotivated 0:9a6f7aafe531 76 }
electromotivated 0:9a6f7aafe531 77
electromotivated 0:9a6f7aafe531 78 float PID::getError(){
electromotivated 0:9a6f7aafe531 79 return error;
electromotivated 0:9a6f7aafe531 80 }
electromotivated 0:9a6f7aafe531 81
electromotivated 0:9a6f7aafe531 82 void PID::set_parameters(float kp, float ki, float kd, float Ts){
electromotivated 0:9a6f7aafe531 83 stop(); // Disable Sample Interrupt... stop()
electromotivated 0:9a6f7aafe531 84 _Ts = Ts; // Set New Sample Time
electromotivated 0:9a6f7aafe531 85 _kp = kp; // Seet New Kp
electromotivated 0:9a6f7aafe531 86 _ki = ki*Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 87 _kd = kd / Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 88 start(); // Enable Sample Interrupt... start()
electromotivated 0:9a6f7aafe531 89 }
electromotivated 0:9a6f7aafe531 90
electromotivated 1:c307cd559154 91 float PID::getKp(){
electromotivated 1:c307cd559154 92 return _kp;
electromotivated 1:c307cd559154 93 }
electromotivated 1:c307cd559154 94
electromotivated 1:c307cd559154 95 float PID::getKi(){
electromotivated 4:4ed1f5bccac8 96 return _ki/_Ts; // Remove Sample time adjustment so that
electromotivated 4:4ed1f5bccac8 97 // actual set ki is returned...
electromotivated 4:4ed1f5bccac8 98 // Remember Sample time is rolled into
electromotivated 4:4ed1f5bccac8 99 // ki inside this class
electromotivated 1:c307cd559154 100 }
electromotivated 1:c307cd559154 101
electromotivated 1:c307cd559154 102 float PID::getKd(){
electromotivated 4:4ed1f5bccac8 103 return _kd*_Ts; // Remove Sample time adjustment so that
electromotivated 4:4ed1f5bccac8 104 // actual set kd is returned...
electromotivated 4:4ed1f5bccac8 105 // Remember Sample time is rolled into
electromotivated 4:4ed1f5bccac8 106 // kd inside this class
electromotivated 1:c307cd559154 107 }
electromotivated 1:c307cd559154 108
electromotivated 1:c307cd559154 109 float PID::getTs(){
electromotivated 1:c307cd559154 110 return _Ts;
electromotivated 1:c307cd559154 111 }
electromotivated 1:c307cd559154 112
electromotivated 0:9a6f7aafe531 113 void PID::sample(){
electromotivated 0:9a6f7aafe531 114 error = *_setpoint - *_feedback;
electromotivated 0:9a6f7aafe531 115
electromotivated 0:9a6f7aafe531 116 // Accumulate Integral Term such ki is applied to current error
electromotivated 0:9a6f7aafe531 117 // before adding to pool; avoids bumps if ki gain value is changed.
electromotivated 0:9a6f7aafe531 118 i_accumulator += _ki * error;
electromotivated 0:9a6f7aafe531 119 // Avoid "Windup" by clamping intergral term to output limits;
electromotivated 0:9a6f7aafe531 120 // essentially we stop integrating when we reach an upper or
electromotivated 0:9a6f7aafe531 121 // lower bound.
electromotivated 0:9a6f7aafe531 122 i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 123
electromotivated 0:9a6f7aafe531 124 // Run it!
electromotivated 0:9a6f7aafe531 125 *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
electromotivated 0:9a6f7aafe531 126 last_feedback = *_feedback;
electromotivated 0:9a6f7aafe531 127 // Clamp Output
electromotivated 0:9a6f7aafe531 128 *_output = clip(*_output, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 129 }
electromotivated 0:9a6f7aafe531 130
electromotivated 0:9a6f7aafe531 131 float PID::clip(float value, float lower, float upper){
electromotivated 0:9a6f7aafe531 132 return std::max(lower, std::min(value, upper));
electromotivated 0:9a6f7aafe531 133 }