Diff: PID.cpp

Revision:

0:35a0b92fe34e

Child:

1:6939c241c6dc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Sat Aug 06 05:31:34 2016 +0000
@@ -0,0 +1,47 @@
+#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;
+}
\ No newline at end of file