Nishant Shah
/
4180-Project-Target
Target code for 4180 Project
Diff: main.cpp
- Revision:
- 0:6bd3fec95c53
diff -r 000000000000 -r 6bd3fec95c53 main.cpp --- /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); + } +}