tugboat project
Dependencies: HMC5883L MMA8451Q PwmIn TinyGPS2 mbed
Fork of tugboat by
Diff: main.cpp
- Revision:
- 4:7fb44cbc97a3
- Parent:
- 2:db76adcdf799
- Child:
- 5:ce6688d37d12
--- a/main.cpp Wed Jul 31 15:37:16 2013 +0000 +++ b/main.cpp Thu Sep 05 14:36:25 2013 +0000 @@ -2,6 +2,7 @@ #include "MMA8451Q.h" #include "TinyGPS.h" #include "HMC5883L.h" +#include "PwmIn.h" #define MMA8451_I2C_ADDRESS (0x1d<<1) @@ -13,14 +14,28 @@ Timer t; TinyGPS gps; Serial gpsSerial(PTD3,PTD2); - +PwmIn rudderIn(PTD0); +PwmIn thrusterIn(PTD5); +PwmOut rudderOut(PTA4); +PwmOut thrusterOut(PTA5); void gpsSerialRecvInterrupt (void); +void radioSerialRecvInterrupt (void); +void radioFlush(void); + +bool manual = true; +int commandLength = 2; +//radio commands +enum{ thruster='a', + rudder, + combo, + control}; + +float rudderC = 0.0015, thrusterC = 0.0015; int main(void) { - pc.baud(115200); radio.baud(57600); int16_t dCompass[3]; @@ -28,8 +43,12 @@ compass.init(); pc.printf("inited\r\n"); gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq); // Recv interrupt handler + radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler gps.reset_ready(); - + rudderOut.period(0.02); + thrusterOut.period(0.02); + rudderOut.pulsewidth(0.0015); + thrusterOut.pulsewidth(0.0015); @@ -47,7 +66,22 @@ 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]); - wait(0.5); + if(manual){ + wait(0.2); + rudderOut.pulsewidth(rudderIn.pulsewidth()); + thrusterOut.pulsewidth(thrusterIn.pulsewidth()); + } + else{ + wait(0.2); + //rudderC should be an angle -45 to 45 + //map this to 0.0013 to 0.0017 + if(rudderC > -45 && rudderC < 45){ + rudderOut.pulsewidth(0.0013+0.004*((rudderC+45)/90)); + } + //thrusterC should be an percentage from 0 to 100 + //map this to 0.0013 to 0.0017 + thrusterOut.pulsewidth(0.0013 + 0.004*(thrusterC/100)); + } } } @@ -64,4 +98,44 @@ //} -} \ No newline at end of file +} +void radioSerialRecvInterrupt (void) +{ + + int firstByte, secondByte; + //get command + 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(); + } + } + +} + +void radioFlush(void) { + while (radio.readable()) { + radio.getc(); + } + return; +}