Diff: main.cpp

Revision:

0:9d0c8058c974

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 19 11:14:04 2020 +0000
@@ -0,0 +1,80 @@
+#include "mbed.h"
+#include "QEI.h"
+
+#define Limit 30.0
+#define PI 3.14159265359
+
+int count_r;
+float rev;
+float rev_back = 0.0;
+float an_vel;
+float powe_element = 0.0;
+float erorr;
+float erorr_back = 0.0;
+float goal = 2.0 * PI;
+float k_p = 20.0;
+float k_i = 3.0;
+float k_d = 2.5;
+float integ;
+float deriva;
+int power;
+
+Ticker warikomi;
+
+int dir;
+//int vel = 130;
+
+Serial pc(USBTX, USBRX, 115200);
+Serial saber(PC_12, PD_2);
+Timer t;
+QEI motor(PC_5, PA_12, NC, 100, &t, QEI::X4_ENCODING);
+
+
+void func()
+{
+    count_r = motor.getPulses();
+    rev = (count_r / 400.0) * 2.0* PI;
+    an_vel = (rev - rev_back) / 0.01f;
+    rev_back = rev;
+
+    //////////////////////////////////
+
+    erorr = goal - rev;
+    integ += (erorr + erorr_back) * (1/2) * 0.01f;
+    deriva = (erorr - erorr_back) / 0.01f;
+    powe_element = k_p * erorr + k_i * integ + k_d * deriva;
+    erorr_back = erorr;
+
+    /////////////////////////////////////////////////////////
+
+    if (31.0 >= powe_element && powe_element >= 0.0) {
+        dir = 0;
+    } else if (powe_element < 0.0) {
+        dir = 1; 
+    }
+
+    power = powe_element / 31.0f * Limit;
+    power = (int)power;
+    
+    if (power > 30){
+        power = 30;
+        }
+
+    /////////////////////////////////////
+
+    saber.putc(130);//アドレス
+    saber.putc(dir);//回転方向
+    saber.putc(abs(power));//出力0~127
+    saber.putc((abs(power) + dir + 130) &0b1111111);
+}
+
+int main()
+{
+    warikomi.attach(&func, 0.01);
+    saber.baud(115200);
+    while(1) {
+        pc.printf("count: %d  count(float): %f   rev: %f  an_vel: %f  powe_element: %f  power: %d  dir: %d  erorr: %f\n"
+                  , count_r, (float)count_r, rev, an_vel, powe_element, power, dir, erorr);
+                  wait(0.01);
+    }
+}
\ No newline at end of file