PID.cpp

00001 #include "PID.h"
00002 /*
00003     Bryce Williams 11/19/2015
00004     See PID.h for references as well as method descriptions
00005 */
00006 
00007 PID::PID(float* setpoint, float* feedback, float* output,
00008          float output_lower, float output_upper,
00009          float  kp, float ki,  float kd, float Ts){
00010     _Ts = Ts;           // Init params
00011     _kp = kp; 
00012     _ki = ki*Ts;        // Roll sample time into gain  
00013     _kd = kd / Ts;      // Roll sample time into gain
00014     
00015     _setpoint = setpoint; 
00016     _feedback = feedback; 
00017     _output = output;
00018     
00019     _output_lower = output_lower;
00020     _output_upper = output_upper;
00021 }
00022 
00023 void PID::start(){
00024     // Start up such we avoid bumps... (see "Initialization" section in
00025     // the reference link found in the header file).
00026     last_feedback = *_feedback;         // Eliminate derivative kick at start/restart
00027     i_accumulator = clip(*_output, _output_lower,
00028                      _output_upper);    // P and D terms are zero, thus 
00029                                         // i term is used to keep output unchanged
00030     /*
00031         If Ki is set to zero we must "flush" the intergral accumulator. 
00032         
00033         Reason:
00034         If we don't "flush", i_accumulator will hold the output value from line
00035         above, and because Ki is now zero only zeros will be added to 
00036         i_accumulator in the sample method, and thus i_accumulator is left 
00037         unchanged from here on out. i_accumulator is now a constant of value
00038         output from line above and will ALWAYS appear in the output. i.e.
00039         
00040         Here is the BUG if we DON'T FLUSH
00041         
00042         _ki = 0;            // User set ki = zero using  PID::set_parameters()
00043         
00044         THEN when PID::set_parameters() calls PID::start() (this method)
00045         
00046         i_accumulator = output;     // From line above
00047         
00048         Then when PID::sample() is called everytime...
00049         
00050         sample(){
00051             i_accumulator += _ki * error;   // Now this is equivalent to 
00052                                             // i_accumulator = output + 0
00053                                             // which always equals output
00054                                             // value from line above
00055 
00056             i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
00057             
00058             // Run it!
00059             *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
00060                 // i_accumulator is fixed at value output from line above
00061                 // i.e. i_accumulator = clip(*_output, _output_lower,
00062                                               _output_upper) = output;
00063             last_feedback = *_feedback;
00064             // Clamp Output
00065             *_output = clip(*_output, _output_lower, _output_upper);
00066                 // Here *_output will always be offset by "output" value
00067                 // from line above 
00068         }   
00069     */
00070     if(-0.00001 <= _ki && _ki <= 0.00001) i_accumulator = 0;
00071     sample_timer.attach(this, &PID::sample, _Ts);
00072 }
00073 
00074 void PID::stop(){
00075     sample_timer.detach();
00076 }
00077 
00078 float PID::getError(){
00079     return error;
00080 }
00081 
00082 void PID::set_parameters(float kp, float ki, float kd, float Ts){
00083     stop();         // Disable Sample Interrupt... stop()
00084     _Ts = Ts;       // Set New Sample Time
00085     _kp = kp;       // Seet New Kp
00086     _ki = ki*Ts;    // Roll sample time into gain  
00087     _kd = kd / Ts;  // Roll sample time into gain
00088     start();        // Enable Sample Interrupt... start()
00089 }
00090 
00091 float PID::getKp(){
00092     return _kp;
00093 }
00094 
00095 float PID::getKi(){
00096     return _ki/_Ts;  // Remove Sample time adjustment so that 
00097                     // actual set ki is returned...
00098                     // Remember Sample time is rolled into 
00099                     // ki inside this class
00100 }
00101 
00102 float PID::getKd(){
00103     return _kd*_Ts;  // Remove Sample time adjustment so that 
00104                     // actual set kd is returned...
00105                     // Remember Sample time is rolled into 
00106                     // kd inside this class
00107 }
00108 
00109 float PID::getTs(){
00110     return _Ts;
00111 }
00112 
00113 void PID::sample(){
00114     error = *_setpoint - *_feedback;
00115 
00116     // Accumulate Integral Term such ki is applied to current error
00117     // before adding to pool; avoids bumps if ki gain value is changed.
00118     i_accumulator += _ki * error;
00119     // Avoid "Windup" by clamping intergral term to output limits;
00120     // essentially we stop integrating when we reach an upper or 
00121     // lower bound.
00122     i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
00123     
00124     // Run it!
00125     *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
00126     last_feedback = *_feedback;
00127     // Clamp Output
00128     *_output = clip(*_output, _output_lower, _output_upper);
00129 }
00130 
00131 float PID::clip(float value, float lower, float upper){
00132     return std::max(lower, std::min(value, upper));
00133 }