Diff: Pid.cpp

Revision:

0:630126b8994f

Child:

1:4bc4c63ea283

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pid.cpp	Wed Dec 19 15:46:32 2018 +0000
@@ -0,0 +1,46 @@
+#include "Pid.h"
+
+void Pid::setup(float Kp,float Ki,float Kd,short PidMode,float period) {
+    KP = Kp;
+    KI = Ki;
+    KD = Kd;
+    MODE = PidMode;
+    PERIOD = period;
+    //pid.attach(this,&Pid::calculate,period);
+}
+
+void Pid::calculate(float targetValue,float nowValue) {
+    switch(MODE) {
+        case 0:
+            before = now;
+            now = targetValue - nowValue;
+            p = now;
+            i += (now + before) * PERIOD / 2;           //積分
+            d = (now - before) / PERIOD;                //微分
+            //target_duty = p * KP + i * KI + d * KD;
+            duty = p * KP + i * KI + d * KD;
+            break;
+        case 1:
+            e2 = e1;
+            e1 = e;
+            e = targetValue - nowValue;
+            //p = (e - e1) / PERIOD;
+            p = e / PERIOD;
+            i = e;
+            d = (e - 2*e1 + e2)/PERIOD;
+            duty += p * KP + i * KI + d * KD;           //dutyは微分されたものなので、積分してあげる(もとに戻す)
+            break;
+        default:
+            break;
+    }
+    
+    if(duty > 0.95f) {
+        duty = 0.95f;
+    } else if (duty < -0.95f) {
+        duty = -0.95f;
+    }
+};
+
+float Pid::getDuty() {
+    return duty;
+}