Diff: pid.cpp

Revision:

0:4ecdddd912e2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.cpp	Sun Sep 24 05:23:58 2017 +0000
@@ -0,0 +1,62 @@
+#include "pid.h"
+
+void PositionPid::setup(float Kp, float Ki, float Kd, float dt){
+    kp = Kp;
+    ki = Ki;
+    kd = Kd;
+    time = dt;
+    frequency = 1/dt;
+}
+
+void PositionPid::calculate(float target, float nowValue){
+    old = now;
+    now = nowValue - target;
+    p = now;
+    i = i + (now + old)*0.5*time;
+    d = (now - old) * frequency;
+    result = kp*p + ki*i + kd*d;
+    if (result > 1.0f) {
+        result = 1.0f;
+    } else if (result < -1.0f) {
+        result = -1.0f;
+    }
+}
+
+float PositionPid::duty(){
+    return result;
+}
+
+float PositionPid::duty_enableWidth(float duty_min, float duty_max){
+    if(now > duty_min && now < duty_max){   //nowは差
+        return 0.0;
+    }else{
+        return result;
+    }
+}
+
+void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){
+    kp = Kp;
+    ki = Ki;
+    kd = Kd;
+    time = dt;
+    frequency = 1/dt;
+}
+
+void SpeedPid::calculate(float target, float nowValue){
+    e2 = e1;
+    e1 = e;
+    e = nowValue - target;
+    p = e - e1;
+    i = e*time;
+    d = (e-2*e1+e2)*frequency;
+    result = result + (kp*p+ki*i+kd*d);
+    if (result > 1.0f) {
+        result = 1.0f;
+    } else if (result < -1.0f) {
+        result = -1.0f;
+    }
+}
+
+float SpeedPid::duty(){
+    return result;
+}