Ryosuke Matsushima / PID

Dependents:   TUKUBAMotorDriver

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?

UserRevisionLine numberNew 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 };