Diff: main.cpp

Revision:

0:6bd3fec95c53

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 11 22:15:16 2016 +0000
@@ -0,0 +1,87 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "string"
+
+//AnalogIn light(p18);
+ 
+RawSerial  pc(USBTX, USBRX);
+RawSerial  dev(p9,p10);
+//DigitalOut led4(LED4);
+ 
+Motor m1(p21, p5, p6);
+Motor m2(p22, p7, p8);
+
+DigitalIn i2(p12);
+DigitalIn i3(p13);
+DigitalIn i4(p14);
+DigitalIn i5(p15);
+DigitalIn i6(p16);
+DigitalIn i7(p17);
+DigitalIn i8(p18);
+DigitalIn i9(p19);
+DigitalIn i10(p20);
+DigitalIn i11(p29);
+DigitalIn i12(p28);
+DigitalIn i13(p27);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+void dev_recv()
+{
+    led1 = !led1;
+    while(dev.readable()) {
+        if(dev.getc() == 'm') {
+            char xbuf[6];
+            char ybuf[6];
+            
+            int ind = 0;
+            char cur = dev.getc();
+            while(cur != ',') {
+                xbuf[ind++] = cur;
+                cur = dev.getc();
+            }
+            xbuf[ind] = NULL;
+            
+            ind = 0;
+            cur = dev.getc();
+            while(cur != '|') {
+                ybuf[ind++] = cur;
+                cur = dev.getc();
+            }
+            ybuf[ind] = NULL;
+            
+            float x, y;
+            x = (float)atof(xbuf);
+            y = (float)atof(ybuf);
+            
+            pc.printf("%f, %f\n",x , y);
+            
+            if (x >= 0.1) {
+                m1.speed( (y + x) );
+                m2.speed(y);
+    //            pc.printf("GREATER THAN %f, %f\n",(y + x) , y);
+            } else if (x <= -0.1) {
+                m1.speed(y);
+                m2.speed( (y - x));
+    //            pc.printf("LESS THAN %f, %f\n",y, (y - x));
+            } else {
+                m1.speed(y);
+                m2.speed(y);
+    //            pc.printf("%.2f, %.2f\n",x, y);
+            }
+        }
+    }
+}
+
+int main() {
+    dev.attach(&dev_recv, Serial::RxIrq);
+    while(1) {
+//        pc.printf("%f\n", i5.read());
+        if(i2 || i3 || i4 || i5 || i6 || i7 || i8 || i9 || i10 || i11 || i12 || i13) {
+            dev.putc('1');
+        }
+        led1 = i2 || i3 || i4 || i5 || i6 || i7 || i8 || i9 || i10 || i11 || i12 || i13;      
+        wait(0.5);
+    }
+}