tugboat project
Dependencies: HMC5883L MMA8451Q PwmIn TinyGPS2 mbed
Fork of tugboat by
main.cpp@9:f56a92eb3e5c, 2013-09-18 (annotated)
- Committer:
- bclaus
- Date:
- Wed Sep 18 20:28:33 2013 +0000
- Revision:
- 9:f56a92eb3e5c
- Parent:
- 8:2917c5f310f7
update rate changed to 10 Hz for everything but gps which is still at 1 Hz
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bclaus | 0:ee158c8007b4 | 1 | #include "mbed.h" |
bclaus | 0:ee158c8007b4 | 2 | #include "MMA8451Q.h" |
bclaus | 2:db76adcdf799 | 3 | #include "TinyGPS.h" |
bclaus | 0:ee158c8007b4 | 4 | #include "HMC5883L.h" |
bclaus | 4:7fb44cbc97a3 | 5 | #include "PwmIn.h" |
bclaus | 0:ee158c8007b4 | 6 | |
bclaus | 0:ee158c8007b4 | 7 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
bclaus | 0:ee158c8007b4 | 8 | |
bclaus | 0:ee158c8007b4 | 9 | I2C i2c_bus(PTE0, PTE1); |
bclaus | 0:ee158c8007b4 | 10 | MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); |
bclaus | 0:ee158c8007b4 | 11 | HMC5883L compass(i2c_bus); |
bclaus | 0:ee158c8007b4 | 12 | Serial pc(USBTX, USBRX); |
bclaus | 0:ee158c8007b4 | 13 | Serial radio(PTC4, PTC3); |
bclaus | 0:ee158c8007b4 | 14 | Timer t; |
bclaus | 2:db76adcdf799 | 15 | TinyGPS gps; |
bclaus | 2:db76adcdf799 | 16 | Serial gpsSerial(PTD3,PTD2); |
bclaus | 7:e8a77af1b428 | 17 | PwmIn thrusterIn(PTD5); |
bclaus | 7:e8a77af1b428 | 18 | PwmIn rudderIn(PTD0); |
bclaus | 7:e8a77af1b428 | 19 | PwmOut rudderOut(PTA5); |
bclaus | 7:e8a77af1b428 | 20 | PwmOut thrusterOut(PTA4); |
bclaus | 2:db76adcdf799 | 21 | |
bclaus | 2:db76adcdf799 | 22 | void gpsSerialRecvInterrupt (void); |
bclaus | 4:7fb44cbc97a3 | 23 | void radioSerialRecvInterrupt (void); |
bclaus | 4:7fb44cbc97a3 | 24 | void radioFlush(void); |
bclaus | 4:7fb44cbc97a3 | 25 | |
bclaus | 4:7fb44cbc97a3 | 26 | bool manual = true; |
bclaus | 4:7fb44cbc97a3 | 27 | int commandLength = 2; |
bclaus | 4:7fb44cbc97a3 | 28 | //radio commands |
bclaus | 4:7fb44cbc97a3 | 29 | enum{ thruster='a', |
bclaus | 4:7fb44cbc97a3 | 30 | rudder, |
bclaus | 4:7fb44cbc97a3 | 31 | combo, |
bclaus | 4:7fb44cbc97a3 | 32 | control}; |
bclaus | 4:7fb44cbc97a3 | 33 | |
bclaus | 7:e8a77af1b428 | 34 | int rudderC = 0, thrusterC = 0; |
bclaus | 0:ee158c8007b4 | 35 | |
bclaus | 0:ee158c8007b4 | 36 | int main(void) { |
bclaus | 8:2917c5f310f7 | 37 | float tLoop=0; |
bclaus | 7:e8a77af1b428 | 38 | float rTime=0.00155,tTime=0.00165; |
bclaus | 0:ee158c8007b4 | 39 | pc.baud(115200); |
bclaus | 2:db76adcdf799 | 40 | radio.baud(57600); |
bclaus | 5:ce6688d37d12 | 41 | |
bclaus | 5:ce6688d37d12 | 42 | pc.printf("pre - inited\r\n"); |
bclaus | 5:ce6688d37d12 | 43 | radio.printf("pre - inited\r\n"); |
bclaus | 0:ee158c8007b4 | 44 | int16_t dCompass[3]; |
bclaus | 0:ee158c8007b4 | 45 | float dAcc[3]; |
bclaus | 8:2917c5f310f7 | 46 | i2c_bus.frequency(1000000); |
bclaus | 0:ee158c8007b4 | 47 | compass.init(); |
bclaus | 2:db76adcdf799 | 48 | pc.printf("inited\r\n"); |
bclaus | 2:db76adcdf799 | 49 | gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq); // Recv interrupt handler |
bclaus | 7:e8a77af1b428 | 50 | radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler |
bclaus | 2:db76adcdf799 | 51 | gps.reset_ready(); |
bclaus | 4:7fb44cbc97a3 | 52 | rudderOut.period(0.02); |
bclaus | 4:7fb44cbc97a3 | 53 | thrusterOut.period(0.02); |
bclaus | 7:e8a77af1b428 | 54 | rudderOut.pulsewidth(0.0016); |
bclaus | 7:e8a77af1b428 | 55 | thrusterOut.pulsewidth(0.00165); |
bclaus | 0:ee158c8007b4 | 56 | |
bclaus | 0:ee158c8007b4 | 57 | |
bclaus | 7:e8a77af1b428 | 58 | t.start(); |
bclaus | 0:ee158c8007b4 | 59 | while (true) { |
bclaus | 7:e8a77af1b428 | 60 | tLoop =t.read(); |
bclaus | 8:2917c5f310f7 | 61 | if(gps.gga_ready() && gps.rmc_ready()){ |
bclaus | 2:db76adcdf799 | 62 | |
bclaus | 7:e8a77af1b428 | 63 | //pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon()); |
bclaus | 8:2917c5f310f7 | 64 | 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()); |
bclaus | 8:2917c5f310f7 | 65 | gps.reset_ready(); |
bclaus | 0:ee158c8007b4 | 66 | } |
bclaus | 0:ee158c8007b4 | 67 | |
bclaus | 0:ee158c8007b4 | 68 | acc.getAccAllAxis(dAcc); |
bclaus | 7:e8a77af1b428 | 69 | //pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]); |
bclaus | 7:e8a77af1b428 | 70 | radio.printf("ACC: %.4f, %.4f, %.4f, %.4f\r\n", t.read(),dAcc[0], dAcc[1], dAcc[2]); |
bclaus | 0:ee158c8007b4 | 71 | compass.getXYZ(dCompass); |
bclaus | 7:e8a77af1b428 | 72 | //pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]); |
bclaus | 7:e8a77af1b428 | 73 | radio.printf("MAG: %.4f, %4d, %4d, %4d\r\n", t.read(),dCompass[0], dCompass[1], dCompass[2]); |
bclaus | 8:2917c5f310f7 | 74 | |
bclaus | 8:2917c5f310f7 | 75 | if(manual){ |
bclaus | 8:2917c5f310f7 | 76 | //low pass filter the rudder time to stop the jitter |
bclaus | 8:2917c5f310f7 | 77 | rTime=rTime + 0.8*(rudderIn.pulsewidth()-rTime); |
bclaus | 8:2917c5f310f7 | 78 | //if outside limits center the rudder |
bclaus | 8:2917c5f310f7 | 79 | if(rTime < 0.001 || rTime > 0.002)rTime=0.00155; |
bclaus | 8:2917c5f310f7 | 80 | tTime =tTime +0.2*(thrusterIn.pulsewidth()-tTime); |
bclaus | 8:2917c5f310f7 | 81 | //if outside limits thruster to zero |
bclaus | 8:2917c5f310f7 | 82 | if(tTime < 0.001 || tTime > 0.002)tTime=0.00165; |
bclaus | 8:2917c5f310f7 | 83 | |
bclaus | 8:2917c5f310f7 | 84 | rudderOut.pulsewidth(rTime); |
bclaus | 8:2917c5f310f7 | 85 | thrusterOut.pulsewidth(tTime); |
bclaus | 8:2917c5f310f7 | 86 | radio.printf("VEH:%.4f, %.6f, %.6f\r\n", t.read(),tTime, rTime); |
bclaus | 8:2917c5f310f7 | 87 | } |
bclaus | 8:2917c5f310f7 | 88 | else{ |
bclaus | 8:2917c5f310f7 | 89 | //rudderC should be an angle -45 to 45 |
bclaus | 8:2917c5f310f7 | 90 | //map this to 0.00135 to 0.00175 |
bclaus | 8:2917c5f310f7 | 91 | rTime=0.0014+0.003*((rudderC+45.0)/90.0); |
bclaus | 8:2917c5f310f7 | 92 | //thrusterC should be an percentage from -100 to 100 |
bclaus | 8:2917c5f310f7 | 93 | //map this to 0.0011 to 0.0019 |
bclaus | 8:2917c5f310f7 | 94 | //0.0019 is full reverse |
bclaus | 8:2917c5f310f7 | 95 | //0.0011 is full ahead |
bclaus | 8:2917c5f310f7 | 96 | tTime=0.0019 - 0.008*((thrusterC+100.0)/200.0); |
bclaus | 8:2917c5f310f7 | 97 | |
bclaus | 8:2917c5f310f7 | 98 | radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC); |
bclaus | 8:2917c5f310f7 | 99 | if(rTime < 0.001 || rTime > 0.002)rTime=0.00155; |
bclaus | 8:2917c5f310f7 | 100 | if(tTime < 0.001 || tTime > 0.002)tTime=0.00165; |
bclaus | 8:2917c5f310f7 | 101 | rudderOut.pulsewidth(rTime); |
bclaus | 8:2917c5f310f7 | 102 | |
bclaus | 8:2917c5f310f7 | 103 | thrusterOut.pulsewidth(tTime); |
bclaus | 8:2917c5f310f7 | 104 | radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC); |
bclaus | 8:2917c5f310f7 | 105 | } |
bclaus | 8:2917c5f310f7 | 106 | //make sure this doesn't run too fast |
bclaus | 8:2917c5f310f7 | 107 | if((t.read()-tLoop)<0.1){ |
bclaus | 8:2917c5f310f7 | 108 | wait(0.1-(t.read()-tLoop)); |
bclaus | 8:2917c5f310f7 | 109 | } |
bclaus | 6:2d7e4561625c | 110 | |
bclaus | 0:ee158c8007b4 | 111 | } |
bclaus | 2:db76adcdf799 | 112 | } |
bclaus | 2:db76adcdf799 | 113 | |
bclaus | 2:db76adcdf799 | 114 | //***************************************************************************** |
bclaus | 2:db76adcdf799 | 115 | // serial receive interrupt handler |
bclaus | 2:db76adcdf799 | 116 | //***************************************************************************** |
bclaus | 2:db76adcdf799 | 117 | |
bclaus | 2:db76adcdf799 | 118 | void gpsSerialRecvInterrupt (void) |
bclaus | 2:db76adcdf799 | 119 | { |
bclaus | 2:db76adcdf799 | 120 | |
bclaus | 2:db76adcdf799 | 121 | |
bclaus | 2:db76adcdf799 | 122 | // while(gpsSerial.readable()){ |
bclaus | 2:db76adcdf799 | 123 | gps.encode(gpsSerial.getc()); |
bclaus | 2:db76adcdf799 | 124 | //} |
bclaus | 2:db76adcdf799 | 125 | |
bclaus | 2:db76adcdf799 | 126 | |
bclaus | 4:7fb44cbc97a3 | 127 | } |
bclaus | 4:7fb44cbc97a3 | 128 | void radioSerialRecvInterrupt (void) |
bclaus | 4:7fb44cbc97a3 | 129 | { |
bclaus | 4:7fb44cbc97a3 | 130 | |
bclaus | 7:e8a77af1b428 | 131 | int firstByte=0; |
bclaus | 7:e8a77af1b428 | 132 | int command='a'; |
bclaus | 7:e8a77af1b428 | 133 | |
bclaus | 7:e8a77af1b428 | 134 | if(radio.readable())firstByte=radio.getc(); |
bclaus | 7:e8a77af1b428 | 135 | |
bclaus | 7:e8a77af1b428 | 136 | if(command=='t'){ |
bclaus | 7:e8a77af1b428 | 137 | |
bclaus | 7:e8a77af1b428 | 138 | thrusterC = firstByte-128; |
bclaus | 7:e8a77af1b428 | 139 | //limit thrusterC |
bclaus | 7:e8a77af1b428 | 140 | if(thrusterC>100){thrusterC=100;} |
bclaus | 7:e8a77af1b428 | 141 | if(thrusterC<-100){thrusterC=-100;} |
bclaus | 7:e8a77af1b428 | 142 | command=0; |
bclaus | 7:e8a77af1b428 | 143 | } |
bclaus | 7:e8a77af1b428 | 144 | else if(command=='r'){ |
bclaus | 7:e8a77af1b428 | 145 | |
bclaus | 7:e8a77af1b428 | 146 | rudderC=firstByte-128; |
bclaus | 7:e8a77af1b428 | 147 | //limit rudderC |
bclaus | 7:e8a77af1b428 | 148 | if(rudderC > 45){rudderC=45;} |
bclaus | 7:e8a77af1b428 | 149 | if(rudderC<-45){rudderC=-45;} |
bclaus | 7:e8a77af1b428 | 150 | command=0; |
bclaus | 7:e8a77af1b428 | 151 | } |
bclaus | 7:e8a77af1b428 | 152 | |
bclaus | 7:e8a77af1b428 | 153 | if(manual==true){ |
bclaus | 7:e8a77af1b428 | 154 | //get command |
bclaus | 7:e8a77af1b428 | 155 | if(firstByte=='a'){ |
bclaus | 7:e8a77af1b428 | 156 | manual=false; |
bclaus | 7:e8a77af1b428 | 157 | command='a'; |
bclaus | 7:e8a77af1b428 | 158 | } |
bclaus | 7:e8a77af1b428 | 159 | }else{ |
bclaus | 7:e8a77af1b428 | 160 | //check if override |
bclaus | 7:e8a77af1b428 | 161 | if(firstByte=='b'){ |
bclaus | 7:e8a77af1b428 | 162 | manual=true; |
bclaus | 7:e8a77af1b428 | 163 | command='b'; |
bclaus | 7:e8a77af1b428 | 164 | }else if(firstByte=='r'){ |
bclaus | 7:e8a77af1b428 | 165 | //take in 0-255 as -45 to 45 where 128 is 0 |
bclaus | 7:e8a77af1b428 | 166 | |
bclaus | 7:e8a77af1b428 | 167 | command='r'; |
bclaus | 7:e8a77af1b428 | 168 | |
bclaus | 7:e8a77af1b428 | 169 | }else if(firstByte=='t'){ |
bclaus | 7:e8a77af1b428 | 170 | |
bclaus | 7:e8a77af1b428 | 171 | command=='t'; |
bclaus | 4:7fb44cbc97a3 | 172 | } |
bclaus | 5:ce6688d37d12 | 173 | |
bclaus | 7:e8a77af1b428 | 174 | |
bclaus | 7:e8a77af1b428 | 175 | } |
bclaus | 7:e8a77af1b428 | 176 | |
bclaus | 7:e8a77af1b428 | 177 | |
bclaus | 4:7fb44cbc97a3 | 178 | |
bclaus | 4:7fb44cbc97a3 | 179 | } |
bclaus | 4:7fb44cbc97a3 | 180 | |
bclaus | 4:7fb44cbc97a3 | 181 | void radioFlush(void) { |
bclaus | 4:7fb44cbc97a3 | 182 | while (radio.readable()) { |
bclaus | 4:7fb44cbc97a3 | 183 | radio.getc(); |
bclaus | 4:7fb44cbc97a3 | 184 | } |
bclaus | 4:7fb44cbc97a3 | 185 | return; |
bclaus | 4:7fb44cbc97a3 | 186 | } |