aaaaaaaaaaaaaaaaaaaaa

Dependencies:   mbed QEI2

Files at this revision

API Documentation at this revision

Comitter:
keiji0604
Date:
Thu Nov 19 11:14:04 2020 +0000
Commit message:
aaaaaaaaaaaaaaaaaa; ; ; ; ; ; ; ;

Changed in this revision

QEI2.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 9d0c8058c974 QEI2.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI2.lib	Thu Nov 19 11:14:04 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/kikoaac/code/QEI2/#49fa8827718d
diff -r 000000000000 -r 9d0c8058c974 main.cpp
--- /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
diff -r 000000000000 -r 9d0c8058c974 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Nov 19 11:14:04 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file