![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
tugboat project
Dependencies: TinyGPS HMC5883L MMA8451Q mbed PwmIn
main.cpp
- Committer:
- bclaus
- Date:
- 2013-09-05
- Revision:
- 4:7fb44cbc97a3
- Parent:
- 2:db76adcdf799
- Child:
- 5:ce6688d37d12
File content as of revision 4:7fb44cbc97a3:
#include "mbed.h" #include "MMA8451Q.h" #include "TinyGPS.h" #include "HMC5883L.h" #include "PwmIn.h" #define MMA8451_I2C_ADDRESS (0x1d<<1) I2C i2c_bus(PTE0, PTE1); MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); HMC5883L compass(i2c_bus); Serial pc(USBTX, USBRX); Serial radio(PTC4, PTC3); 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]; float dAcc[3]; 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); while (true) { if(gps.gga_ready()){ pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon()); radio.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon()); } acc.getAccAllAxis(dAcc); pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]); radio.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]); 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(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)); } } } //***************************************************************************** // serial receive interrupt handler //***************************************************************************** void gpsSerialRecvInterrupt (void) { // while(gpsSerial.readable()){ gps.encode(gpsSerial.getc()); //} } 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; }