tugboat project
Dependencies: TinyGPS HMC5883L MMA8451Q mbed PwmIn
main.cpp@6:2d7e4561625c, 2013-09-05 (annotated)
- Committer:
- bclaus
- Date:
- Thu Sep 05 18:55:20 2013 +0000
- Revision:
- 6:2d7e4561625c
- Parent:
- 5:ce6688d37d12
- Child:
- 7:e8a77af1b428
slightly more owrking;
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 | 6:2d7e4561625c | 17 | PwmIn rudderIn(PTD5); |
bclaus | 6:2d7e4561625c | 18 | PwmIn thrusterIn(PTD0); |
bclaus | 4:7fb44cbc97a3 | 19 | PwmOut rudderOut(PTA4); |
bclaus | 4:7fb44cbc97a3 | 20 | PwmOut thrusterOut(PTA5); |
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 | 4:7fb44cbc97a3 | 34 | float rudderC = 0.0015, thrusterC = 0.0015; |
bclaus | 0:ee158c8007b4 | 35 | |
bclaus | 0:ee158c8007b4 | 36 | int main(void) { |
bclaus | 0:ee158c8007b4 | 37 | |
bclaus | 6:2d7e4561625c | 38 | int firstByte, secondByte; |
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 | 0:ee158c8007b4 | 46 | compass.init(); |
bclaus | 2:db76adcdf799 | 47 | pc.printf("inited\r\n"); |
bclaus | 2:db76adcdf799 | 48 | gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq); // Recv interrupt handler |
bclaus | 6:2d7e4561625c | 49 | //radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler |
bclaus | 2:db76adcdf799 | 50 | gps.reset_ready(); |
bclaus | 4:7fb44cbc97a3 | 51 | rudderOut.period(0.02); |
bclaus | 4:7fb44cbc97a3 | 52 | thrusterOut.period(0.02); |
bclaus | 4:7fb44cbc97a3 | 53 | rudderOut.pulsewidth(0.0015); |
bclaus | 4:7fb44cbc97a3 | 54 | thrusterOut.pulsewidth(0.0015); |
bclaus | 0:ee158c8007b4 | 55 | |
bclaus | 0:ee158c8007b4 | 56 | |
bclaus | 0:ee158c8007b4 | 57 | |
bclaus | 0:ee158c8007b4 | 58 | while (true) { |
bclaus | 0:ee158c8007b4 | 59 | |
bclaus | 2:db76adcdf799 | 60 | if(gps.gga_ready()){ |
bclaus | 2:db76adcdf799 | 61 | |
bclaus | 2:db76adcdf799 | 62 | pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon()); |
bclaus | 2:db76adcdf799 | 63 | radio.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon()); |
bclaus | 0:ee158c8007b4 | 64 | } |
bclaus | 0:ee158c8007b4 | 65 | |
bclaus | 0:ee158c8007b4 | 66 | acc.getAccAllAxis(dAcc); |
bclaus | 0:ee158c8007b4 | 67 | pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]); |
bclaus | 0:ee158c8007b4 | 68 | radio.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]); |
bclaus | 0:ee158c8007b4 | 69 | compass.getXYZ(dCompass); |
bclaus | 0:ee158c8007b4 | 70 | pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]); |
bclaus | 0:ee158c8007b4 | 71 | radio.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]); |
bclaus | 6:2d7e4561625c | 72 | |
bclaus | 6:2d7e4561625c | 73 | if(radio.readable()){ |
bclaus | 6:2d7e4561625c | 74 | firstByte = radio.getc(); |
bclaus | 6:2d7e4561625c | 75 | |
bclaus | 6:2d7e4561625c | 76 | if(firstByte == 'c'){ |
bclaus | 6:2d7e4561625c | 77 | manual = false; |
bclaus | 6:2d7e4561625c | 78 | secondByte = radio.getc(); |
bclaus | 6:2d7e4561625c | 79 | switch(secondByte){ |
bclaus | 6:2d7e4561625c | 80 | case thruster: |
bclaus | 6:2d7e4561625c | 81 | //example command 'ca 50' |
bclaus | 6:2d7e4561625c | 82 | radio.scanf(" %f",thrusterC); |
bclaus | 6:2d7e4561625c | 83 | break; |
bclaus | 6:2d7e4561625c | 84 | case rudder: |
bclaus | 6:2d7e4561625c | 85 | //example command 'cb -20' |
bclaus | 6:2d7e4561625c | 86 | radio.scanf(" %f",rudderC); |
bclaus | 6:2d7e4561625c | 87 | break; |
bclaus | 6:2d7e4561625c | 88 | case combo: |
bclaus | 6:2d7e4561625c | 89 | //example command 'cc -20,50' |
bclaus | 6:2d7e4561625c | 90 | radio.scanf(" %f,%f",rudderC,thrusterC); |
bclaus | 6:2d7e4561625c | 91 | break; |
bclaus | 6:2d7e4561625c | 92 | case control: |
bclaus | 6:2d7e4561625c | 93 | //example command 'cd' |
bclaus | 6:2d7e4561625c | 94 | manual = true; |
bclaus | 6:2d7e4561625c | 95 | break; |
bclaus | 6:2d7e4561625c | 96 | default: |
bclaus | 6:2d7e4561625c | 97 | //shouldn't be here flush stream |
bclaus | 6:2d7e4561625c | 98 | radioFlush(); |
bclaus | 6:2d7e4561625c | 99 | } |
bclaus | 6:2d7e4561625c | 100 | } |
bclaus | 6:2d7e4561625c | 101 | else{ |
bclaus | 6:2d7e4561625c | 102 | //we shouldnt be here do nothing |
bclaus | 6:2d7e4561625c | 103 | |
bclaus | 6:2d7e4561625c | 104 | } |
bclaus | 6:2d7e4561625c | 105 | } |
bclaus | 6:2d7e4561625c | 106 | |
bclaus | 4:7fb44cbc97a3 | 107 | if(manual){ |
bclaus | 6:2d7e4561625c | 108 | wait(0.5); |
bclaus | 4:7fb44cbc97a3 | 109 | rudderOut.pulsewidth(rudderIn.pulsewidth()); |
bclaus | 4:7fb44cbc97a3 | 110 | thrusterOut.pulsewidth(thrusterIn.pulsewidth()); |
bclaus | 4:7fb44cbc97a3 | 111 | } |
bclaus | 4:7fb44cbc97a3 | 112 | else{ |
bclaus | 6:2d7e4561625c | 113 | wait(0.5); |
bclaus | 4:7fb44cbc97a3 | 114 | //rudderC should be an angle -45 to 45 |
bclaus | 4:7fb44cbc97a3 | 115 | //map this to 0.0013 to 0.0017 |
bclaus | 4:7fb44cbc97a3 | 116 | if(rudderC > -45 && rudderC < 45){ |
bclaus | 4:7fb44cbc97a3 | 117 | rudderOut.pulsewidth(0.0013+0.004*((rudderC+45)/90)); |
bclaus | 4:7fb44cbc97a3 | 118 | } |
bclaus | 4:7fb44cbc97a3 | 119 | //thrusterC should be an percentage from 0 to 100 |
bclaus | 4:7fb44cbc97a3 | 120 | //map this to 0.0013 to 0.0017 |
bclaus | 4:7fb44cbc97a3 | 121 | thrusterOut.pulsewidth(0.0013 + 0.004*(thrusterC/100)); |
bclaus | 4:7fb44cbc97a3 | 122 | } |
bclaus | 0:ee158c8007b4 | 123 | } |
bclaus | 2:db76adcdf799 | 124 | } |
bclaus | 2:db76adcdf799 | 125 | |
bclaus | 2:db76adcdf799 | 126 | //***************************************************************************** |
bclaus | 2:db76adcdf799 | 127 | // serial receive interrupt handler |
bclaus | 2:db76adcdf799 | 128 | //***************************************************************************** |
bclaus | 2:db76adcdf799 | 129 | |
bclaus | 2:db76adcdf799 | 130 | void gpsSerialRecvInterrupt (void) |
bclaus | 2:db76adcdf799 | 131 | { |
bclaus | 2:db76adcdf799 | 132 | |
bclaus | 2:db76adcdf799 | 133 | |
bclaus | 2:db76adcdf799 | 134 | // while(gpsSerial.readable()){ |
bclaus | 2:db76adcdf799 | 135 | gps.encode(gpsSerial.getc()); |
bclaus | 2:db76adcdf799 | 136 | //} |
bclaus | 2:db76adcdf799 | 137 | |
bclaus | 2:db76adcdf799 | 138 | |
bclaus | 4:7fb44cbc97a3 | 139 | } |
bclaus | 4:7fb44cbc97a3 | 140 | void radioSerialRecvInterrupt (void) |
bclaus | 4:7fb44cbc97a3 | 141 | { |
bclaus | 4:7fb44cbc97a3 | 142 | |
bclaus | 4:7fb44cbc97a3 | 143 | int firstByte, secondByte; |
bclaus | 4:7fb44cbc97a3 | 144 | //get command |
bclaus | 6:2d7e4561625c | 145 | |
bclaus | 4:7fb44cbc97a3 | 146 | firstByte = radio.getc(); |
bclaus | 4:7fb44cbc97a3 | 147 | if(firstByte == 'c'){ |
bclaus | 4:7fb44cbc97a3 | 148 | manual = false; |
bclaus | 4:7fb44cbc97a3 | 149 | secondByte = radio.getc(); |
bclaus | 4:7fb44cbc97a3 | 150 | switch(secondByte){ |
bclaus | 4:7fb44cbc97a3 | 151 | case thruster: |
bclaus | 4:7fb44cbc97a3 | 152 | //example command 'ca 50' |
bclaus | 4:7fb44cbc97a3 | 153 | radio.scanf(" %f",thrusterC); |
bclaus | 4:7fb44cbc97a3 | 154 | break; |
bclaus | 4:7fb44cbc97a3 | 155 | case rudder: |
bclaus | 4:7fb44cbc97a3 | 156 | //example command 'cb -20' |
bclaus | 4:7fb44cbc97a3 | 157 | radio.scanf(" %f",rudderC); |
bclaus | 4:7fb44cbc97a3 | 158 | break; |
bclaus | 4:7fb44cbc97a3 | 159 | case combo: |
bclaus | 4:7fb44cbc97a3 | 160 | //example command 'cc -20,50' |
bclaus | 4:7fb44cbc97a3 | 161 | radio.scanf(" %f,%f",rudderC,thrusterC); |
bclaus | 4:7fb44cbc97a3 | 162 | break; |
bclaus | 4:7fb44cbc97a3 | 163 | case control: |
bclaus | 4:7fb44cbc97a3 | 164 | //example command 'cd' |
bclaus | 4:7fb44cbc97a3 | 165 | manual = true; |
bclaus | 4:7fb44cbc97a3 | 166 | break; |
bclaus | 4:7fb44cbc97a3 | 167 | default: |
bclaus | 4:7fb44cbc97a3 | 168 | //shouldn't be here flush stream |
bclaus | 4:7fb44cbc97a3 | 169 | radioFlush(); |
bclaus | 4:7fb44cbc97a3 | 170 | } |
bclaus | 4:7fb44cbc97a3 | 171 | } |
bclaus | 5:ce6688d37d12 | 172 | else{ |
bclaus | 5:ce6688d37d12 | 173 | //we shouldnt be here do nothing |
bclaus | 5:ce6688d37d12 | 174 | |
bclaus | 5:ce6688d37d12 | 175 | } |
bclaus | 4:7fb44cbc97a3 | 176 | |
bclaus | 4:7fb44cbc97a3 | 177 | } |
bclaus | 4:7fb44cbc97a3 | 178 | |
bclaus | 4:7fb44cbc97a3 | 179 | void radioFlush(void) { |
bclaus | 4:7fb44cbc97a3 | 180 | while (radio.readable()) { |
bclaus | 4:7fb44cbc97a3 | 181 | radio.getc(); |
bclaus | 4:7fb44cbc97a3 | 182 | } |
bclaus | 4:7fb44cbc97a3 | 183 | return; |
bclaus | 4:7fb44cbc97a3 | 184 | } |