tugboat project
Dependencies: HMC5883L MMA8451Q PwmIn TinyGPS2 mbed
Fork of tugboat by
Diff: main.cpp
- Revision:
- 8:2917c5f310f7
- Parent:
- 7:e8a77af1b428
--- a/main.cpp Thu Sep 12 13:57:36 2013 +0000 +++ b/main.cpp Wed Sep 18 20:26:20 2013 +0000 @@ -34,7 +34,7 @@ int rudderC = 0, thrusterC = 0; int main(void) { - float tLoop=0,tLoopi=0; + float tLoop=0; float rTime=0.00155,tTime=0.00165; pc.baud(115200); radio.baud(57600); @@ -43,6 +43,7 @@ radio.printf("pre - inited\r\n"); int16_t dCompass[3]; float dAcc[3]; + i2c_bus.frequency(1000000); compass.init(); pc.printf("inited\r\n"); gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq); // Recv interrupt handler @@ -57,10 +58,11 @@ t.start(); while (true) { tLoop =t.read(); - if(gps.gga_ready()){ + if(gps.gga_ready() && gps.rmc_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()); + radio.printf("GPS: %.4f, %.8f, %.8f, %.4f, %.4f, %ul, %ul, %ul, %ul\r\n", t.read(),gps.f_lat(), gps.f_lon(),gps.f_course(),gps.f_speed_knots(),gps.ul_time(),gps.ul_date(),gps.sat_count(),gps.hdop()); + gps.reset_ready(); } acc.getAccAllAxis(dAcc); @@ -69,45 +71,43 @@ 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]); + + 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()-tLoop)<0.1){ + wait(0.1-(t.read()-tLoop)); + } - 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)); - } - } } } @@ -169,8 +169,6 @@ }else if(firstByte=='t'){ command=='t'; - }else{ - //do nothing, this will eat all the garbage chars... }