Diff: main.cpp

Revision:

0:753587765f3d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 27 01:48:59 2018 +0000
@@ -0,0 +1,55 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "define.h" //ステータス用
+
+Ticker timer;
+Timer T;
+
+QEI Enc1(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+QEI Enc2(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+DigitalIn sw(p26);
+Serial Saber(p13,p14);
+Serial pc(USBTX,USBRX);
+
+int encount1 = 0, encount2 = 0;
+float  encount_rot = 0;
+float spd1 = 0;
+
+void timer_warikomi()
+{
+    static float pre_sp1 = 0;
+    float pull_ratio = 1.0;
+    float multi = 4.0;
+    float ppr = 1.0;
+    static int cmd = 15;
+    encount1 = Enc1.getPulses();
+    encount2 = Enc2.getPulses();
+    if (encount1 > encount2) encount_rot = encount1;
+    else encount_rot = encount2;
+    
+    float rot_sp1 = encount_rot/multi/ppr*pull_ratio; 
+    spd1 = (rot_sp1 - pre_sp1)/INT_TIME/(48*4);
+        
+        if(sw == ON) cmd = 0;
+        
+        Saber.putc(SB_ADRS);
+        Saber.putc(1);
+        Saber.putc(cmd);
+        Saber.putc((SB_ADRS + 1 + cmd) & 0b01111111);
+        
+        Saber.putc(SB_ADRS);
+        Saber.putc(4);
+        Saber.putc(cmd);
+        Saber.putc((SB_ADRS + 4 + cmd) & 0b01111111);
+        pre_sp1 = rot_sp1;
+}
+
+int main() {
+    Saber.baud(115200);
+    pc.baud(9600);
+    timer.attach(timer_warikomi,INT_TIME);
+    
+    while(1) {
+    pc.printf("%d\t%d\t%f\t%f\n", encount1 , encount2, encount_rot, spd1);
+    }
+}