hidaka sato / PID

Ultimate Gain method

Dependents:   2016_slave_MD_rorikon 2016_slave_MD_rorikon WRS_mechanamu_test

PID.cpp@4:3ca1603fbcda, 2016-09-30 (annotated)

Committer:
sgrsn
Date:
Fri Sep 30 12:31:16 2016 +0000
Revision:
4:3ca1603fbcda
Parent:
3:b4a401daed8a
Child:
5:6949e401a9ad
PID

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 4:3ca1603fbcda 10 lateD = 0;
sgrsn 0:35a0b92fe34e 11 timer.start();
sgrsn 0:35a0b92fe34e 12 }
sgrsn 0:35a0b92fe34e 13 float PID::control(float target, float nowrpm)
sgrsn 0:35a0b92fe34e 14 {
sgrsn 0:35a0b92fe34e 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 0:35a0b92fe34e 23 prev_time = timer.read();
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 4:3ca1603fbcda 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 4:3ca1603fbcda 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 0:35a0b92fe34e 38 float PID::control_P(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 4:3ca1603fbcda 44 float PID::control_PI(float target, float nowrpm)
sgrsn 4:3ca1603fbcda 45 {
sgrsn 4:3ca1603fbcda 46 Kp = 0.45 * Ku;
sgrsn 4:3ca1603fbcda 47 Ti = 0.83 * Pu;
sgrsn 4:3ca1603fbcda 48 Ki = (1 / Ti) * Kp;
sgrsn 4:3ca1603fbcda 49 nowtime = timer.read();
sgrsn 4:3ca1603fbcda 50 float hensa = target - nowrpm;
sgrsn 4:3ca1603fbcda 51 float dt = nowtime - prev_time;
sgrsn 4:3ca1603fbcda 52 integral += (hensa + prev_hensa) / 2 * dt;
sgrsn 4:3ca1603fbcda 53 float sousaryou = Kp*hensa + Ki*integral;
sgrsn 4:3ca1603fbcda 54 //printf("%f %f %f\r\n", Kp*hensa, Ki*integral, Kd*differential);
sgrsn 4:3ca1603fbcda 55 prev_hensa = hensa;
sgrsn 4:3ca1603fbcda 56 prev_time = timer.read();
sgrsn 4:3ca1603fbcda 57 return sousaryou;
sgrsn 4:3ca1603fbcda 58 }
sgrsn 0:35a0b92fe34e 59 void PID::set_parameter(float new_Ku, float new_Pu)
sgrsn 1:6939c241c6dc 60 {
sgrsn 0:35a0b92fe34e 61 Ku = new_Ku;
sgrsn 0:35a0b92fe34e 62 Pu = new_Pu;
sgrsn 0:35a0b92fe34e 63
sgrsn 0:35a0b92fe34e 64 Kp = 0.60 * Ku;
sgrsn 0:35a0b92fe34e 65 Ti = 0.50 * Pu;
sgrsn 0:35a0b92fe34e 66 Td = 0.125 * Pu;
sgrsn 0:35a0b92fe34e 67 Ki = (1 / Ti) * Kp;
sgrsn 0:35a0b92fe34e 68 Kd = Td * Kp;
sgrsn 0:35a0b92fe34e 69 }
sgrsn 0:35a0b92fe34e 70 void PID::reset()
sgrsn 0:35a0b92fe34e 71 {
sgrsn 0:35a0b92fe34e 72 integral = 0;
sgrsn 0:35a0b92fe34e 73 prev_hensa = 0;
sgrsn 3:b4a401daed8a 74 prev_time = timer.read();
sgrsn 4:3ca1603fbcda 75 }