tugboat project
Dependencies: HMC5883L MMA8451Q PwmIn TinyGPS2 mbed
Fork of tugboat by
main.cpp
- Committer:
- bclaus
- Date:
- 2013-09-12
- Revision:
- 7:e8a77af1b428
- Parent:
- 6:2d7e4561625c
- Child:
- 8:2917c5f310f7
File content as of revision 7:e8a77af1b428:
#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 thrusterIn(PTD5); PwmIn rudderIn(PTD0); PwmOut rudderOut(PTA5); PwmOut thrusterOut(PTA4); void gpsSerialRecvInterrupt (void); void radioSerialRecvInterrupt (void); void radioFlush(void); bool manual = true; int commandLength = 2; //radio commands enum{ thruster='a', rudder, combo, control}; int rudderC = 0, thrusterC = 0; int main(void) { float tLoop=0,tLoopi=0; float rTime=0.00155,tTime=0.00165; pc.baud(115200); radio.baud(57600); pc.printf("pre - inited\r\n"); radio.printf("pre - inited\r\n"); 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.0016); thrusterOut.pulsewidth(0.00165); t.start(); while (true) { tLoop =t.read(); if(gps.gga_ready()){ //pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon()); radio.printf("GPS: %.4f, %.8f, %.8f, %.4f, %.4f\r\n", t.read(),gps.f_lat(), gps.f_lon(),gps.f_course(),gps.f_speed_knots()); } acc.getAccAllAxis(dAcc); //pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]); radio.printf("ACC: %.4f, %.4f, %.4f, %.4f\r\n", t.read(),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("MAG: %.4f, %4d, %4d, %4d\r\n", t.read(),dCompass[0], dCompass[1], dCompass[2]); while(t.read() - tLoop < 0.5){ tLoopi=t.read(); if(manual){ //low pass filter the rudder time to stop the jitter rTime=rTime + 0.8*(rudderIn.pulsewidth()-rTime); //if outside limits center the rudder if(rTime < 0.001 || rTime > 0.002)rTime=0.00155; tTime =tTime +0.2*(thrusterIn.pulsewidth()-tTime); //if outside limits thruster to zero if(tTime < 0.001 || tTime > 0.002)tTime=0.00165; rudderOut.pulsewidth(rTime); thrusterOut.pulsewidth(tTime); radio.printf("VEH:%.4f, %.6f, %.6f\r\n", t.read(),tTime, rTime); } else{ //rudderC should be an angle -45 to 45 //map this to 0.00135 to 0.00175 rTime=0.0014+0.003*((rudderC+45.0)/90.0); //thrusterC should be an percentage from -100 to 100 //map this to 0.0011 to 0.0019 //0.0019 is full reverse //0.0011 is full ahead tTime=0.0019 - 0.008*((thrusterC+100.0)/200.0); radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC); if(rTime < 0.001 || rTime > 0.002)rTime=0.00155; if(tTime < 0.001 || tTime > 0.002)tTime=0.00165; rudderOut.pulsewidth(rTime); thrusterOut.pulsewidth(tTime); radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC); } //make sure this doesn't run too fast if((t.read()-tLoopi)<0.1){ wait(0.1-(t.read()-tLoopi)); } } } } //***************************************************************************** // serial receive interrupt handler //***************************************************************************** void gpsSerialRecvInterrupt (void) { // while(gpsSerial.readable()){ gps.encode(gpsSerial.getc()); //} } void radioSerialRecvInterrupt (void) { int firstByte=0; int command='a'; if(radio.readable())firstByte=radio.getc(); if(command=='t'){ thrusterC = firstByte-128; //limit thrusterC if(thrusterC>100){thrusterC=100;} if(thrusterC<-100){thrusterC=-100;} command=0; } else if(command=='r'){ rudderC=firstByte-128; //limit rudderC if(rudderC > 45){rudderC=45;} if(rudderC<-45){rudderC=-45;} command=0; } if(manual==true){ //get command if(firstByte=='a'){ manual=false; command='a'; } }else{ //check if override if(firstByte=='b'){ manual=true; command='b'; }else if(firstByte=='r'){ //take in 0-255 as -45 to 45 where 128 is 0 command='r'; }else if(firstByte=='t'){ command=='t'; }else{ //do nothing, this will eat all the garbage chars... } } } void radioFlush(void) { while (radio.readable()) { radio.getc(); } return; }