Diff: PID.cpp

Revision:

4:3ca1603fbcda

Parent:

3:b4a401daed8a

Child:

5:6949e401a9ad

--- a/PID.cpp	Thu Aug 18 02:41:53 2016 +0000
+++ b/PID.cpp	Fri Sep 30 12:31:16 2016 +0000
@@ -7,6 +7,7 @@
     prev_hensa  = 0;
     nowtime     = 0;
     prev_time   = 0;
+    lateD      = 0;
     timer.start();
 }
 float PID::control(float target, float nowrpm)
@@ -17,16 +18,44 @@
     integral += (hensa + prev_hensa) / 2 * dt;
     float differential = (hensa - prev_hensa) / dt;
     float sousaryou = Kp*hensa + Ki*integral + Kd*differential;
+    //printf("%f %f %f\r\n", Kp*hensa, Ki*integral, Kd*differential);
     prev_hensa = hensa;
     prev_time = timer.read();
     return sousaryou;
 }
+float PID::PI_lateD(float target, float nowrpm)
+{
+    nowtime = timer.read();
+    float hensa = target - nowrpm;
+    float dt = nowtime - prev_time;
+    integral += (hensa + prev_hensa) / 2 * dt;
+    float sousaryou = Kp*hensa + Ki*integral + Kd*lateD;
+    prev_hensa = hensa;
+    prev_time = timer.read();
+    lateD = (hensa - prev_hensa) / dt;
+    return sousaryou;
+}
 float PID::control_P(float target, float nowrpm, float new_Kp)
 {
     float hensa = target - nowrpm;
     float sousaryou = new_Kp*hensa;
     return sousaryou;
 }
+float PID::control_PI(float target, float nowrpm)
+{
+    Kp = 0.45 * Ku;
+    Ti = 0.83 * Pu;
+    Ki = (1 / Ti) * Kp;
+    nowtime = timer.read();
+    float hensa = target - nowrpm;
+    float dt = nowtime - prev_time;
+    integral += (hensa + prev_hensa) / 2 * dt;
+    float sousaryou = Kp*hensa + Ki*integral;
+    //printf("%f %f %f\r\n", Kp*hensa, Ki*integral, Kd*differential);
+    prev_hensa = hensa;
+    prev_time = timer.read();
+    return sousaryou;
+}
 void PID::set_parameter(float new_Ku, float new_Pu)
 {  
     Ku = new_Ku;
@@ -43,4 +72,4 @@
     integral    = 0;
     prev_hensa  = 0;
     prev_time = timer.read();
-}
\ No newline at end of file
+}