Diff: main.cpp

Revision:

0:0e3b9c903282

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 01 05:57:40 2021 +0000
@@ -0,0 +1,84 @@
+#include "mbed.h"
+#include "RoboClaw.h"
+#include "QEI.h"
+#include "Filter.h"
+
+Filter filter(0.01);
+Timer t;
+QEI motor(p21, p22, NC, 500, &t, QEI::X4_ENCODING);
+RoboClaw robo(128, 115200, p9, NC);
+Serial pc(USBTX, USBRX, 115200);
+Ticker flipper;
+
+int count_r;
+int pre_count = 0;
+int error_PID;
+int goal = 3000;
+
+double k_p = 0.00035;
+double k_i = 0.00075;
+double k_d = 0.0003;
+
+double pro;
+double integ;
+double diff;
+double s_t = 0.01;
+
+double def;
+int s_d = 0.0;
+
+bool flag_check = false;
+bool flag_printf = false;
+
+int count_t = 0;
+
+void func()
+{
+    //goal = (int)filter.SecondOrderLag(3000);
+    count_t++;
+    count_r = motor.getPulses();
+    error_PID = (double)(goal - count_r);
+    integ += (double)(count_r + pre_count) / 2.0 * s_t;
+    diff = (count_r - pre_count) / s_t;
+    def = k_p * error_PID + k_i * integ + k_d * diff;
+
+    s_d = (int)def;
+    if(s_d > 30) {
+        s_d = 30;
+    }
+
+    if(s_d < -30) {
+        s_d = -30;
+    }
+
+    flag_check = true;
+
+    if(count_t >= 10) flag_printf = true;
+
+    pre_count = count_r;
+}
+
+int main()
+{
+    filter.setSecondOrderPara(1.0, 1.0, 0.0);
+    flipper.attach(&func, 0.01);
+
+    while(1) {
+
+        if (flag_check == true) {
+            if (error_PID <= 0) {
+                robo.ForwardM2(s_d);
+                flag_check = false;
+            } else {
+                robo.BackwardM2(-1 * s_d);
+                flag_check = false;
+            }
+        }
+        if(flag_printf == true) {
+            pc.printf("count_r : %d\t\terror : %d\t\ts_d : %d\t\tgoal : %d\n", count_r, error_PID, s_d, goal);
+            flag_printf = false;
+            count_r = 0;
+        }
+
+    }
+}
\ No newline at end of file