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?
User | Revision | Line number | New 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 | } |