Ryosuke Matsushima / PID

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.

Dependents:   TUKUBAMotorDriver

PID.cpp@1:e2fb92398c1f, 2020-12-30 (annotated)

Committer:
ryosukenanoda
Date:
Wed Dec 30 17:30:24 2020 +0000
Revision:
1:e2fb92398c1f
Parent:
0:4b3a2875ad1d
Child:
2:0413525bbb2e
fix direction

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 integral_val += (last_val * val) / 2 * dt;
ryosukenanoda 0:4b3a2875ad1d 24 float d_val = (val - last_val) / dt;
ryosukenanoda 1:e2fb92398c1f 25 last_time = now_time;
ryosukenanoda 1:e2fb92398c1f 26 last_val = val;
ryosukenanoda 0:4b3a2875ad1d 27
ryosukenanoda 0:4b3a2875ad1d 28 return _p * val+ _i * integral_val + _d * d_val;
ryosukenanoda 0:4b3a2875ad1d 29 };
ryosukenanoda 0:4b3a2875ad1d 30
ryosukenanoda 0:4b3a2875ad1d 31 void PID::set_target(float val) {
ryosukenanoda 0:4b3a2875ad1d 32 target_val = val;
ryosukenanoda 0:4b3a2875ad1d 33 reset_integral_val();
ryosukenanoda 0:4b3a2875ad1d 34 };
ryosukenanoda 0:4b3a2875ad1d 35
ryosukenanoda 0:4b3a2875ad1d 36 void PID::reset_integral_val() {
ryosukenanoda 0:4b3a2875ad1d 37 integral_val = 0.0;
ryosukenanoda 0:4b3a2875ad1d 38 };