hidaka sato / PID

Ultimate Gain method

Dependents:   2016_slave_MD_rorikon 2016_slave_MD_rorikon WRS_mechanamu_test

PID.cpp@1:6939c241c6dc, 2016-08-15 (annotated)

Committer:
sgrsn
Date:
Mon Aug 15 04:52:28 2016 +0000
Revision:
1:6939c241c6dc
Parent:
0:35a0b92fe34e
Child:
3:b4a401daed8a
ok

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 0:35a0b92fe34e 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 0:35a0b92fe34e 10 timer.start();
sgrsn 0:35a0b92fe34e 11 }
sgrsn 0:35a0b92fe34e 12 float PID::control(float target, float nowrpm)
sgrsn 0:35a0b92fe34e 13 {
sgrsn 0:35a0b92fe34e 14 nowtime = timer.read();
sgrsn 0:35a0b92fe34e 15 float hensa = target - nowrpm;
sgrsn 0:35a0b92fe34e 16 float dt = nowtime - prev_time;
sgrsn 0:35a0b92fe34e 17 integral += (hensa + prev_hensa) / 2 * dt;
sgrsn 0:35a0b92fe34e 18 float differential = (hensa - prev_hensa) / dt;
sgrsn 0:35a0b92fe34e 19 float sousaryou = Kp*hensa + Ki*integral + Kd*differential;
sgrsn 0:35a0b92fe34e 20 prev_hensa = hensa;
sgrsn 0:35a0b92fe34e 21 prev_time = timer.read();
sgrsn 0:35a0b92fe34e 22 return sousaryou;
sgrsn 0:35a0b92fe34e 23 }
sgrsn 0:35a0b92fe34e 24 float PID::control_P(float target, float nowrpm, float new_Kp)
sgrsn 0:35a0b92fe34e 25 {
sgrsn 0:35a0b92fe34e 26 float hensa = target - nowrpm;
sgrsn 0:35a0b92fe34e 27 float sousaryou = new_Kp*hensa;
sgrsn 0:35a0b92fe34e 28 return sousaryou;
sgrsn 0:35a0b92fe34e 29 }
sgrsn 0:35a0b92fe34e 30 void PID::set_parameter(float new_Ku, float new_Pu)
sgrsn 1:6939c241c6dc 31 {
sgrsn 0:35a0b92fe34e 32 Ku = new_Ku;
sgrsn 0:35a0b92fe34e 33 Pu = new_Pu;
sgrsn 0:35a0b92fe34e 34
sgrsn 0:35a0b92fe34e 35 Kp = 0.60 * Ku;
sgrsn 0:35a0b92fe34e 36 Ti = 0.50 * Pu;
sgrsn 0:35a0b92fe34e 37 Td = 0.125 * Pu;
sgrsn 0:35a0b92fe34e 38 Ki = (1 / Ti) * Kp;
sgrsn 0:35a0b92fe34e 39 Kd = Td * Kp;
sgrsn 0:35a0b92fe34e 40 }
sgrsn 0:35a0b92fe34e 41 void PID::reset()
sgrsn 0:35a0b92fe34e 42 {
sgrsn 0:35a0b92fe34e 43 integral = 0;
sgrsn 0:35a0b92fe34e 44 prev_hensa = 0;
sgrsn 0:35a0b92fe34e 45 prev_time = 0;
sgrsn 0:35a0b92fe34e 46 }