Diff: main.cpp

Revision:

0:262d28994243

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Aug 21 07:20:38 2016 +0000
@@ -0,0 +1,44 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "Locate.h"
+
+#define rPR 100
+#define lPR 100
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+DigitalOut rv(PA_6);
+DigitalOut lv(PA_14);
+DigitalIn  finished(D15);
+
+QEI right(PA_7, PA_5, NC, lPR, QEI::X2_ENCODING);
+QEI left(PA_13, PA_15, NC, lPR, QEI::X2_ENCODING);  //Aを上から7番目に着ける,GNDはA15の下
+
+Locate machine(0,0);
+
+int main()
+{
+    rv = 1;
+    lv = 1;
+
+    int cnt = -10;
+    
+    while(cnt++ < 0)
+    {
+    pc.printf("Rpulse: %d Lpulses: %d\n\r x(mm): %d y(mm): %d theta: %f.\n\r"
+                ,right.getPulses(), left.getPulses(), machine.getX(), machine.getY(), machine.rad);
+    pc.printf("Rpulse: %d Lpulses: %d\n\r x(mm): %d y(mm): %d theta: %f.\n\r"
+                ,-right.getPulses(), left.getPulses(), machine.getX(), machine.getY(), machine.rad);
+    //machine.setup(right.getPulses(),left.getPulses());
+   
+    }    
+    
+    while(1) {
+        machine.update(-right.getPulses(), left.getPulses());
+        if (cnt++ >= 200) {
+            cnt = 0;
+            pc.printf("Rpulse: %d Lpulses: %d x: %f\n\r x(mm): %d y(mm): %d theta: %f.\n\r"
+                ,-right.getPulses(), left.getPulses(), machine.x, machine.getX(), machine.getY(), machine.rad);
+        }
+    }
+}
\ No newline at end of file