Noam Nahum / mypidror1

for dror

Dependents:   NewpidDror Recieverdror Trancieverdror CANrecive

PID.cpp@3:6c2c985408df, 2015-11-24 (annotated)

Committer:
electromotivated
Date:
Tue Nov 24 02:28:24 2015 +0000
Revision:
3:6c2c985408df
Parent:
1:c307cd559154
Child:
4:4ed1f5bccac8
Bug Fix: Flush i_accumulator in start() method to prevent output from hanging. Read comments in start() method in .cpp file for more info

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 1:c307cd559154 96 return _ki;
electromotivated 1:c307cd559154 97 }
electromotivated 1:c307cd559154 98
electromotivated 1:c307cd559154 99 float PID::getKd(){
electromotivated 1:c307cd559154 100 return _kd;
electromotivated 1:c307cd559154 101 }
electromotivated 1:c307cd559154 102
electromotivated 1:c307cd559154 103 float PID::getTs(){
electromotivated 1:c307cd559154 104 return _Ts;
electromotivated 1:c307cd559154 105 }
electromotivated 1:c307cd559154 106
electromotivated 0:9a6f7aafe531 107 void PID::sample(){
electromotivated 0:9a6f7aafe531 108 error = *_setpoint - *_feedback;
electromotivated 0:9a6f7aafe531 109
electromotivated 0:9a6f7aafe531 110 // Accumulate Integral Term such ki is applied to current error
electromotivated 0:9a6f7aafe531 111 // before adding to pool; avoids bumps if ki gain value is changed.
electromotivated 0:9a6f7aafe531 112 i_accumulator += _ki * error;
electromotivated 0:9a6f7aafe531 113 // Avoid "Windup" by clamping intergral term to output limits;
electromotivated 0:9a6f7aafe531 114 // essentially we stop integrating when we reach an upper or
electromotivated 0:9a6f7aafe531 115 // lower bound.
electromotivated 0:9a6f7aafe531 116 i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 117
electromotivated 0:9a6f7aafe531 118 // Run it!
electromotivated 0:9a6f7aafe531 119 *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
electromotivated 0:9a6f7aafe531 120 last_feedback = *_feedback;
electromotivated 0:9a6f7aafe531 121 // Clamp Output
electromotivated 0:9a6f7aafe531 122 *_output = clip(*_output, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 123 }
electromotivated 0:9a6f7aafe531 124
electromotivated 0:9a6f7aafe531 125 float PID::clip(float value, float lower, float upper){
electromotivated 0:9a6f7aafe531 126 return std::max(lower, std::min(value, upper));
electromotivated 0:9a6f7aafe531 127 }