Ultimate Gain method
Dependents: 2016_slave_MD_rorikon 2016_slave_MD_rorikon WRS_mechanamu_test
PID.cpp@0:35a0b92fe34e, 2016-08-06 (annotated)
- Committer:
- sgrsn
- Date:
- Sat Aug 06 05:31:34 2016 +0000
- Revision:
- 0:35a0b92fe34e
- Child:
- 1:6939c241c6dc
Ultimate Gain method
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 | 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 | 0:35a0b92fe34e | 31 | { |
sgrsn | 0:35a0b92fe34e | 32 | |
sgrsn | 0:35a0b92fe34e | 33 | Ku = new_Ku; |
sgrsn | 0:35a0b92fe34e | 34 | Pu = new_Pu; |
sgrsn | 0:35a0b92fe34e | 35 | |
sgrsn | 0:35a0b92fe34e | 36 | Kp = 0.60 * Ku; |
sgrsn | 0:35a0b92fe34e | 37 | Ti = 0.50 * Pu; |
sgrsn | 0:35a0b92fe34e | 38 | Td = 0.125 * Pu; |
sgrsn | 0:35a0b92fe34e | 39 | Ki = (1 / Ti) * Kp; |
sgrsn | 0:35a0b92fe34e | 40 | Kd = Td * Kp; |
sgrsn | 0:35a0b92fe34e | 41 | } |
sgrsn | 0:35a0b92fe34e | 42 | void PID::reset() |
sgrsn | 0:35a0b92fe34e | 43 | { |
sgrsn | 0:35a0b92fe34e | 44 | integral = 0; |
sgrsn | 0:35a0b92fe34e | 45 | prev_hensa = 0; |
sgrsn | 0:35a0b92fe34e | 46 | prev_time = 0; |
sgrsn | 0:35a0b92fe34e | 47 | } |