Diff: pid.cpp

Revision:

0:0d02a451e79d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.cpp	Sun Oct 10 04:47:14 2021 +0000
@@ -0,0 +1,54 @@
+#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.5f * 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;
+}
+
+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;
+}