hidaka sato / PID

Ultimate Gain method

Dependents:   2016_slave_MD_rorikon 2016_slave_MD_rorikon WRS_mechanamu_test

PID.cpp@5:6949e401a9ad, 2018-11-14 (annotated)

Committer:
sgrsn
Date:
Wed Nov 14 02:20:07 2018 +0000
Revision:
5:6949e401a9ad
Parent:
4:3ca1603fbcda 
...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:35a0b92fe34e 1 #include "PID.h"
sgrsn 0:35a0b92fe34e 2
sgrsn 0:35a0b92fe34e 3 PID::PID(Timer *T)
sgrsn 0:35a0b92fe34e 4 {
sgrsn 5:6949e401a9ad 5 timer = T;
sgrsn 0:35a0b92fe34e 6 integral = 0;
sgrsn 0:35a0b92fe34e 7 prev_hensa = 0;
sgrsn 0:35a0b92fe34e 8 nowtime = 0;
sgrsn 0:35a0b92fe34e 9 prev_time = 0;
sgrsn 4:3ca1603fbcda 10 lateD = 0;
sgrsn 5:6949e401a9ad 11 timer->start();
sgrsn 0:35a0b92fe34e 12 }
sgrsn 5:6949e401a9ad 13 float PID::controlPID(float target, float nowrpm)
sgrsn 0:35a0b92fe34e 14 {
sgrsn 5:6949e401a9ad 15 nowtime = timer->read();
sgrsn 0:35a0b92fe34e 16 float hensa = target - nowrpm;
sgrsn 0:35a0b92fe34e 17 float dt = nowtime - prev_time;
sgrsn 0:35a0b92fe34e 18 integral += (hensa + prev_hensa) / 2 * dt;
sgrsn 0:35a0b92fe34e 19 float differential = (hensa - prev_hensa) / dt;
sgrsn 0:35a0b92fe34e 20 float sousaryou = Kp*hensa + Ki*integral + Kd*differential;
sgrsn 4:3ca1603fbcda 21 //printf("%f %f %f\r\n", Kp*hensa, Ki*integral, Kd*differential);
sgrsn 0:35a0b92fe34e 22 prev_hensa = hensa;
sgrsn 5:6949e401a9ad 23 prev_time = nowtime;
sgrsn 0:35a0b92fe34e 24 return sousaryou;
sgrsn 0:35a0b92fe34e 25 }
sgrsn 4:3ca1603fbcda 26 float PID::PI_lateD(float target, float nowrpm)
sgrsn 4:3ca1603fbcda 27 {
sgrsn 5:6949e401a9ad 28 nowtime = timer->read();
sgrsn 4:3ca1603fbcda 29 float hensa = target - nowrpm;
sgrsn 4:3ca1603fbcda 30 float dt = nowtime - prev_time;
sgrsn 4:3ca1603fbcda 31 integral += (hensa + prev_hensa) / 2 * dt;
sgrsn 4:3ca1603fbcda 32 float sousaryou = Kp*hensa + Ki*integral + Kd*lateD;
sgrsn 4:3ca1603fbcda 33 prev_hensa = hensa;
sgrsn 5:6949e401a9ad 34 prev_time = timer->read();
sgrsn 4:3ca1603fbcda 35 lateD = (hensa - prev_hensa) / dt;
sgrsn 4:3ca1603fbcda 36 return sousaryou;
sgrsn 4:3ca1603fbcda 37 }
sgrsn 5:6949e401a9ad 38 float PID::controlP(float target, float nowrpm, float new_Kp)
sgrsn 0:35a0b92fe34e 39 {
sgrsn 0:35a0b92fe34e 40 float hensa = target - nowrpm;
sgrsn 0:35a0b92fe34e 41 float sousaryou = new_Kp*hensa;
sgrsn 0:35a0b92fe34e 42 return sousaryou;
sgrsn 0:35a0b92fe34e 43 }
sgrsn 5:6949e401a9ad 44 float PID::controlPI(float target, float nowrpm)
sgrsn 4:3ca1603fbcda 45 {
sgrsn 5:6949e401a9ad 46 nowtime = timer->read();
sgrsn 4:3ca1603fbcda 47 float hensa = target - nowrpm;
sgrsn 4:3ca1603fbcda 48 float dt = nowtime - prev_time;
sgrsn 4:3ca1603fbcda 49 integral += (hensa + prev_hensa) / 2 * dt;
sgrsn 4:3ca1603fbcda 50 float sousaryou = Kp*hensa + Ki*integral;
sgrsn 4:3ca1603fbcda 51 //printf("%f %f %f\r\n", Kp*hensa, Ki*integral, Kd*differential);
sgrsn 4:3ca1603fbcda 52 prev_hensa = hensa;
sgrsn 5:6949e401a9ad 53 prev_time = timer->read();
sgrsn 4:3ca1603fbcda 54 return sousaryou;
sgrsn 4:3ca1603fbcda 55 }
sgrsn 5:6949e401a9ad 56 void PID::setParameter(float new_Kp, float new_Ki, float new_Kd)
sgrsn 5:6949e401a9ad 57 {
sgrsn 5:6949e401a9ad 58 Kp = new_Kp;
sgrsn 5:6949e401a9ad 59 Ki = new_Ki;
sgrsn 5:6949e401a9ad 60 Kd = new_Kd;
sgrsn 5:6949e401a9ad 61 }
sgrsn 5:6949e401a9ad 62 void PID::setParameter(float new_Ku, float new_Pu)
sgrsn 1:6939c241c6dc 63 {
sgrsn 0:35a0b92fe34e 64 Ku = new_Ku;
sgrsn 0:35a0b92fe34e 65 Pu = new_Pu;
sgrsn 0:35a0b92fe34e 66
sgrsn 0:35a0b92fe34e 67 Kp = 0.60 * Ku;
sgrsn 0:35a0b92fe34e 68 Ti = 0.50 * Pu;
sgrsn 0:35a0b92fe34e 69 Td = 0.125 * Pu;
sgrsn 0:35a0b92fe34e 70 Ki = (1 / Ti) * Kp;
sgrsn 0:35a0b92fe34e 71 Kd = Td * Kp;
sgrsn 0:35a0b92fe34e 72 }
sgrsn 5:6949e401a9ad 73 void PID::setParameterPI(float new_Ku, float new_Pu)
sgrsn 5:6949e401a9ad 74 {
sgrsn 5:6949e401a9ad 75 Kp = 0.45 * Ku;
sgrsn 5:6949e401a9ad 76 Ti = 0.83 * Pu;
sgrsn 5:6949e401a9ad 77 Ki = (1 / Ti) * Kp;
sgrsn 5:6949e401a9ad 78 }
sgrsn 0:35a0b92fe34e 79 void PID::reset()
sgrsn 0:35a0b92fe34e 80 {
sgrsn 0:35a0b92fe34e 81 integral = 0;
sgrsn 0:35a0b92fe34e 82 prev_hensa = 0;
sgrsn 5:6949e401a9ad 83 prev_time = timer->read();
sgrsn 4:3ca1603fbcda 84 }