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@4:4ed1f5bccac8, 2015-11-24 (annotated)
- Committer:
- electromotivated
- Date:
- Tue Nov 24 22:52:45 2015 +0000
- Revision:
- 4:4ed1f5bccac8
- Parent:
- 3:6c2c985408df
Fixed bug in getKi() and getKd() methods. They were returning the Sample time adjusted gains used by the class. Now returns the pure gains as set by user. ;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |