Takumi Nakagawara / 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.

PID.cpp@1:4705d8930670, 2019-04-07 (annotated)

Committer:
Takkun
Date:
Sun Apr 07 13:52:44 2019 +0000
Revision:
1:4705d8930670
Parent:
0:be531c5604db
Child:
2:c8ab3e8d4c51
Add an argument "Timer *_timer" to the constructor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TakumiToda 0:be531c5604db 1 #include "PID.h"
TakumiToda 0:be531c5604db 2 #include "mbed.h"
TakumiToda 0:be531c5604db 3
Takkun 1:4705d8930670 4 PID::PID(Timer *_timer){
TakumiToda 0:be531c5604db 5 integral = 0;
TakumiToda 0:be531c5604db 6 prev_hensa = 0;
TakumiToda 0:be531c5604db 7 nowtime = 0;
TakumiToda 0:be531c5604db 8 prev_time = 0;
TakumiToda 0:be531c5604db 9 lateD = 0;
Takkun 1:4705d8930670 10 timer = _timer;
TakumiToda 0:be531c5604db 11 }
TakumiToda 0:be531c5604db 12
Takkun 1:4705d8930670 13 float PID::control(float target, float nowrpm){
TakumiToda 0:be531c5604db 14 //timer->start();
TakumiToda 0:be531c5604db 15 nowtime =timer->read();
TakumiToda 0:be531c5604db 16 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 17 float dt = 1000000 / (nowtime - prev_time);
TakumiToda 0:be531c5604db 18 integral += (hensa + prev_hensa) / 2 * dt;
TakumiToda 0:be531c5604db 19 float differential = (hensa - prev_hensa) / dt;
TakumiToda 0:be531c5604db 20 float sousaryou = Kp*hensa + Ki*integral + Kd*differential;
TakumiToda 0:be531c5604db 21 prev_hensa = hensa;
TakumiToda 0:be531c5604db 22 prev_time =timer->read();
TakumiToda 0:be531c5604db 23 return sousaryou;
TakumiToda 0:be531c5604db 24 }
TakumiToda 0:be531c5604db 25
Takkun 1:4705d8930670 26 float PID::PI_lateD(float target, float nowrpm){
TakumiToda 0:be531c5604db 27 nowtime = (*timer).read();
TakumiToda 0:be531c5604db 28 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 29 float dt = nowtime - prev_time;
TakumiToda 0:be531c5604db 30 integral += (hensa + prev_hensa) / 2 *dt;
TakumiToda 0:be531c5604db 31 float sousaryou = Kp*hensa + Ki*integral + Kd*lateD;
TakumiToda 0:be531c5604db 32 prev_hensa = hensa;
TakumiToda 0:be531c5604db 33 prev_time = timer->read();
TakumiToda 0:be531c5604db 34 lateD = (hensa - prev_hensa) / dt;
TakumiToda 0:be531c5604db 35 return sousaryou;
TakumiToda 0:be531c5604db 36 }
TakumiToda 0:be531c5604db 37
TakumiToda 0:be531c5604db 38 float PID::control_P(float target, float nowrpm, float new_Kp){
TakumiToda 0:be531c5604db 39 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 40 float sousaryou = new_Kp*hensa;
TakumiToda 0:be531c5604db 41 return sousaryou;
TakumiToda 0:be531c5604db 42 }
TakumiToda 0:be531c5604db 43
Takkun 1:4705d8930670 44 float PID::control_PI(float target, float nowrpm){
TakumiToda 0:be531c5604db 45 Kp = 0.45 * Ku;
TakumiToda 0:be531c5604db 46 Ti = 0.83 * Pu;
TakumiToda 0:be531c5604db 47 Ki = (1 / Ti) * Kp;
TakumiToda 0:be531c5604db 48 nowtime = timer->read();
TakumiToda 0:be531c5604db 49 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 50 float dt = nowtime -prev_time;
TakumiToda 0:be531c5604db 51 integral += (hensa + prev_hensa) / 2 * dt;
TakumiToda 0:be531c5604db 52 float sousaryou = Kp*hensa + Ki*integral;
TakumiToda 0:be531c5604db 53 prev_hensa = hensa;
TakumiToda 0:be531c5604db 54 prev_time = timer->read();
TakumiToda 0:be531c5604db 55 return sousaryou;
TakumiToda 0:be531c5604db 56 }
TakumiToda 0:be531c5604db 57
TakumiToda 0:be531c5604db 58 void PID::setParameter_pid(float new_Kp, float new_Ki, float new_Kd){
TakumiToda 0:be531c5604db 59 Kp = new_Kp;
TakumiToda 0:be531c5604db 60 Ki = new_Ki;
TakumiToda 0:be531c5604db 61 Kd = new_Kd;
TakumiToda 0:be531c5604db 62 }
TakumiToda 0:be531c5604db 63
TakumiToda 0:be531c5604db 64 void PID::setParameter_KuPu(float new_Ku, float new_Pu){
TakumiToda 0:be531c5604db 65 Ku = new_Ku;
TakumiToda 0:be531c5604db 66 Pu = new_Pu;
TakumiToda 0:be531c5604db 67 Kp = 0.60 * Ku;
TakumiToda 0:be531c5604db 68 Ti = 0.50 * Pu;
TakumiToda 0:be531c5604db 69 Td = 0.125 * Pu;
TakumiToda 0:be531c5604db 70 Ki = (1 / Ti) * Kp;
TakumiToda 0:be531c5604db 71 Kd = Td * Kp;
TakumiToda 0:be531c5604db 72 }
TakumiToda 0:be531c5604db 73
TakumiToda 0:be531c5604db 74 void PID::reset(float target){
TakumiToda 0:be531c5604db 75 integral = 0;
TakumiToda 0:be531c5604db 76 prev_hensa = target;
TakumiToda 0:be531c5604db 77 prev_time = timer->read();
TakumiToda 0:be531c5604db 78 }