tugboat project
Dependencies: TinyGPS HMC5883L MMA8451Q mbed PwmIn
Diff: main.cpp
- Revision:
- 6:2d7e4561625c
- Parent:
- 5:ce6688d37d12
- Child:
- 7:e8a77af1b428
diff -r ce6688d37d12 -r 2d7e4561625c main.cpp --- a/main.cpp Thu Sep 05 18:04:18 2013 +0000 +++ b/main.cpp Thu Sep 05 18:55:20 2013 +0000 @@ -14,8 +14,8 @@ Timer t; TinyGPS gps; Serial gpsSerial(PTD3,PTD2); -PwmIn rudderIn(PTD0); -PwmIn thrusterIn(PTD5); +PwmIn rudderIn(PTD5); +PwmIn thrusterIn(PTD0); PwmOut rudderOut(PTA4); PwmOut thrusterOut(PTA5); @@ -35,7 +35,7 @@ int main(void) { - + int firstByte, secondByte; pc.baud(115200); radio.baud(57600); @@ -46,7 +46,7 @@ compass.init(); pc.printf("inited\r\n"); gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq); // Recv interrupt handler - radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler + //radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler gps.reset_ready(); rudderOut.period(0.02); thrusterOut.period(0.02); @@ -69,13 +69,48 @@ compass.getXYZ(dCompass); pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]); radio.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]); + + if(radio.readable()){ + firstByte = radio.getc(); + + if(firstByte == 'c'){ + manual = false; + secondByte = radio.getc(); + switch(secondByte){ + case thruster: + //example command 'ca 50' + radio.scanf(" %f",thrusterC); + break; + case rudder: + //example command 'cb -20' + radio.scanf(" %f",rudderC); + break; + case combo: + //example command 'cc -20,50' + radio.scanf(" %f,%f",rudderC,thrusterC); + break; + case control: + //example command 'cd' + manual = true; + break; + default: + //shouldn't be here flush stream + radioFlush(); + } + } + else{ + //we shouldnt be here do nothing + + } + } + if(manual){ - wait(0.2); + wait(0.5); rudderOut.pulsewidth(rudderIn.pulsewidth()); thrusterOut.pulsewidth(thrusterIn.pulsewidth()); } else{ - wait(0.2); + wait(0.5); //rudderC should be an angle -45 to 45 //map this to 0.0013 to 0.0017 if(rudderC > -45 && rudderC < 45){ @@ -107,6 +142,7 @@ int firstByte, secondByte; //get command + firstByte = radio.getc(); if(firstByte == 'c'){ manual = false;