Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
PID.cpp@2:0413525bbb2e, 2021-01-17 (annotated)
- Committer:
- ryosukenanoda
- Date:
- Sun Jan 17 02:06:07 2021 +0000
- Revision:
- 2:0413525bbb2e
- Parent:
- 1:e2fb92398c1f
add original control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryosukenanoda | 0:4b3a2875ad1d | 1 | #include "mbed.h" |
ryosukenanoda | 0:4b3a2875ad1d | 2 | #include "PID.h" |
ryosukenanoda | 0:4b3a2875ad1d | 3 | |
ryosukenanoda | 0:4b3a2875ad1d | 4 | PID::PID(float p, float i, float d) { |
ryosukenanoda | 0:4b3a2875ad1d | 5 | _p = p; |
ryosukenanoda | 0:4b3a2875ad1d | 6 | _i = i; |
ryosukenanoda | 0:4b3a2875ad1d | 7 | _d = d; |
ryosukenanoda | 0:4b3a2875ad1d | 8 | |
ryosukenanoda | 0:4b3a2875ad1d | 9 | target_val = 0.0; |
ryosukenanoda | 0:4b3a2875ad1d | 10 | integral_val = 0.0; |
ryosukenanoda | 0:4b3a2875ad1d | 11 | last_time = 0; //us |
ryosukenanoda | 0:4b3a2875ad1d | 12 | last_val = 0.0; |
ryosukenanoda | 0:4b3a2875ad1d | 13 | |
ryosukenanoda | 0:4b3a2875ad1d | 14 | timer.start(); |
ryosukenanoda | 0:4b3a2875ad1d | 15 | }; |
ryosukenanoda | 0:4b3a2875ad1d | 16 | |
ryosukenanoda | 0:4b3a2875ad1d | 17 | float PID::get_input(float val) { |
ryosukenanoda | 0:4b3a2875ad1d | 18 | int now_time = timer.read_us(); |
ryosukenanoda | 0:4b3a2875ad1d | 19 | float dt = now_time - last_time; |
ryosukenanoda | 0:4b3a2875ad1d | 20 | dt /= pow(10.0, 6.0); |
ryosukenanoda | 0:4b3a2875ad1d | 21 | |
ryosukenanoda | 1:e2fb92398c1f | 22 | val = target_val - val; |
ryosukenanoda | 0:4b3a2875ad1d | 23 | |
ryosukenanoda | 2:0413525bbb2e | 24 | //// normal PID |
ryosukenanoda | 2:0413525bbb2e | 25 | // integral_val += (last_val * val) / 2 * dt; |
ryosukenanoda | 2:0413525bbb2e | 26 | // float d_val = (val - last_val) / dt; |
ryosukenanoda | 2:0413525bbb2e | 27 | // last_time = now_time; |
ryosukenanoda | 2:0413525bbb2e | 28 | // last_val = val; |
ryosukenanoda | 2:0413525bbb2e | 29 | // return _p * val+ _i * integral_val + _d * d_val; |
ryosukenanoda | 2:0413525bbb2e | 30 | |
ryosukenanoda | 2:0413525bbb2e | 31 | //// original |
ryosukenanoda | 2:0413525bbb2e | 32 | if(abs(integral_val) < 50 || integral_val * val < 0) { |
ryosukenanoda | 2:0413525bbb2e | 33 | integral_val += val * _p; |
ryosukenanoda | 2:0413525bbb2e | 34 | } |
ryosukenanoda | 2:0413525bbb2e | 35 | return integral_val; |
ryosukenanoda | 0:4b3a2875ad1d | 36 | }; |
ryosukenanoda | 0:4b3a2875ad1d | 37 | |
ryosukenanoda | 0:4b3a2875ad1d | 38 | void PID::set_target(float val) { |
ryosukenanoda | 0:4b3a2875ad1d | 39 | target_val = val; |
ryosukenanoda | 0:4b3a2875ad1d | 40 | reset_integral_val(); |
ryosukenanoda | 0:4b3a2875ad1d | 41 | }; |
ryosukenanoda | 0:4b3a2875ad1d | 42 | |
ryosukenanoda | 0:4b3a2875ad1d | 43 | void PID::reset_integral_val() { |
ryosukenanoda | 0:4b3a2875ad1d | 44 | integral_val = 0.0; |
ryosukenanoda | 0:4b3a2875ad1d | 45 | }; |