main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "string"
00004 
00005 //AnalogIn light(p18);
00006  
00007 RawSerial  pc(USBTX, USBRX);
00008 RawSerial  dev(p9,p10);
00009 //DigitalOut led4(LED4);
00010  
00011 Motor m1(p21, p5, p6);
00012 Motor m2(p22, p7, p8);
00013 
00014 DigitalIn i2(p12);
00015 DigitalIn i3(p13);
00016 DigitalIn i4(p14);
00017 DigitalIn i5(p15);
00018 DigitalIn i6(p16);
00019 DigitalIn i7(p17);
00020 DigitalIn i8(p18);
00021 DigitalIn i9(p19);
00022 DigitalIn i10(p20);
00023 DigitalIn i11(p29);
00024 DigitalIn i12(p28);
00025 DigitalIn i13(p27);
00026 
00027 DigitalOut led1(LED1);
00028 DigitalOut led2(LED2);
00029 
00030 void dev_recv()
00031 {
00032     led1 = !led1;
00033     while(dev.readable()) {
00034         if(dev.getc() == 'm') {
00035             char xbuf[6];
00036             char ybuf[6];
00037             
00038             int ind = 0;
00039             char cur = dev.getc();
00040             while(cur != ',') {
00041                 xbuf[ind++] = cur;
00042                 cur = dev.getc();
00043             }
00044             xbuf[ind] = NULL;
00045             
00046             ind = 0;
00047             cur = dev.getc();
00048             while(cur != '|') {
00049                 ybuf[ind++] = cur;
00050                 cur = dev.getc();
00051             }
00052             ybuf[ind] = NULL;
00053             
00054             float x, y;
00055             x = (float)atof(xbuf);
00056             y = (float)atof(ybuf);
00057             
00058             pc.printf("%f, %f\n",x , y);
00059             
00060             if (x >= 0.1) {
00061                 m1.speed( (y + x) );
00062                 m2.speed(y);
00063     //            pc.printf("GREATER THAN %f, %f\n",(y + x) , y);
00064             } else if (x <= -0.1) {
00065                 m1.speed(y);
00066                 m2.speed( (y - x));
00067     //            pc.printf("LESS THAN %f, %f\n",y, (y - x));
00068             } else {
00069                 m1.speed(y);
00070                 m2.speed(y);
00071     //            pc.printf("%.2f, %.2f\n",x, y);
00072             }
00073         }
00074     }
00075 }
00076 
00077 int main() {
00078     dev.attach(&dev_recv, Serial::RxIrq);
00079     while(1) {
00080 //        pc.printf("%f\n", i5.read());
00081         if(i2 || i3 || i4 || i5 || i6 || i7 || i8 || i9 || i10 || i11 || i12 || i13) {
00082             dev.putc('1');
00083         }
00084         led1 = i2 || i3 || i4 || i5 || i6 || i7 || i8 || i9 || i10 || i11 || i12 || i13;      
00085         wait(0.5);
00086     }
00087 }