Ultimate Gain method
Dependents: 2016_slave_MD_rorikon 2016_slave_MD_rorikon WRS_mechanamu_test
PID.cpp
- Committer:
- sgrsn
- Date:
- 2016-08-06
- Revision:
- 0:35a0b92fe34e
- Child:
- 1:6939c241c6dc
File content as of revision 0:35a0b92fe34e:
#include "PID.h" PID::PID(Timer *T) { timer = *T; integral = 0; prev_hensa = 0; nowtime = 0; prev_time = 0; timer.start(); } float PID::control(float target, float nowrpm) { nowtime = timer.read(); float hensa = target - nowrpm; float dt = nowtime - prev_time; integral += (hensa + prev_hensa) / 2 * dt; float differential = (hensa - prev_hensa) / dt; float sousaryou = Kp*hensa + Ki*integral + Kd*differential; prev_hensa = hensa; prev_time = timer.read(); return sousaryou; } float PID::control_P(float target, float nowrpm, float new_Kp) { float hensa = target - nowrpm; float sousaryou = new_Kp*hensa; return sousaryou; } void PID::set_parameter(float new_Ku, float new_Pu) { Ku = new_Ku; Pu = new_Pu; Kp = 0.60 * Ku; Ti = 0.50 * Pu; Td = 0.125 * Pu; Ki = (1 / Ti) * Kp; Kd = Td * Kp; } void PID::reset() { integral = 0; prev_hensa = 0; prev_time = 0; }