brian claus / Mbed 2 deprecated tugboat

tugboat project

Dependencies:   TinyGPS HMC5883L MMA8451Q mbed PwmIn

main.cpp@7:e8a77af1b428, 2013-09-12 (annotated)

Committer:
bclaus
Date:
Thu Sep 12 13:57:36 2013 +0000
Revision:
7:e8a77af1b428
Parent:
6:2d7e4561625c 
Revision 0.  This is the first one that really works;

Who changed what in which revision?

UserRevisionLine numberNew 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 7:e8a77af1b428 37 float tLoop=0,tLoopi=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 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 7:e8a77af1b428 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 7:e8a77af1b428 53 rudderOut.pulsewidth(0.0016);
bclaus 7:e8a77af1b428 54 thrusterOut.pulsewidth(0.00165);
bclaus 0:ee158c8007b4 55
bclaus 0:ee158c8007b4 56
bclaus 7:e8a77af1b428 57 t.start();
bclaus 0:ee158c8007b4 58 while (true) {
bclaus 7:e8a77af1b428 59 tLoop =t.read();
bclaus 2:db76adcdf799 60 if(gps.gga_ready()){
bclaus 2:db76adcdf799 61
bclaus 7:e8a77af1b428 62 //pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
bclaus 7:e8a77af1b428 63 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());
bclaus 0:ee158c8007b4 64 }
bclaus 0:ee158c8007b4 65
bclaus 0:ee158c8007b4 66 acc.getAccAllAxis(dAcc);
bclaus 7:e8a77af1b428 67 //pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
bclaus 7:e8a77af1b428 68 radio.printf("ACC: %.4f, %.4f, %.4f, %.4f\r\n", t.read(),dAcc[0], dAcc[1], dAcc[2]);
bclaus 0:ee158c8007b4 69 compass.getXYZ(dCompass);
bclaus 7:e8a77af1b428 70 //pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
bclaus 7:e8a77af1b428 71 radio.printf("MAG: %.4f, %4d, %4d, %4d\r\n", t.read(),dCompass[0], dCompass[1], dCompass[2]);
bclaus 6:2d7e4561625c 72
bclaus 7:e8a77af1b428 73 while(t.read() - tLoop < 0.5){
bclaus 7:e8a77af1b428 74 tLoopi=t.read();
bclaus 7:e8a77af1b428 75 if(manual){
bclaus 7:e8a77af1b428 76 //low pass filter the rudder time to stop the jitter
bclaus 7:e8a77af1b428 77 rTime=rTime + 0.8*(rudderIn.pulsewidth()-rTime);
bclaus 7:e8a77af1b428 78 //if outside limits center the rudder
bclaus 7:e8a77af1b428 79 if(rTime < 0.001 || rTime > 0.002)rTime=0.00155;
bclaus 7:e8a77af1b428 80 tTime =tTime +0.2*(thrusterIn.pulsewidth()-tTime);
bclaus 7:e8a77af1b428 81 //if outside limits thruster to zero
bclaus 7:e8a77af1b428 82 if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
bclaus 7:e8a77af1b428 83
bclaus 7:e8a77af1b428 84 rudderOut.pulsewidth(rTime);
bclaus 7:e8a77af1b428 85 thrusterOut.pulsewidth(tTime);
bclaus 7:e8a77af1b428 86 radio.printf("VEH:%.4f, %.6f, %.6f\r\n", t.read(),tTime, rTime);
bclaus 6:2d7e4561625c 87 }
bclaus 6:2d7e4561625c 88 else{
bclaus 7:e8a77af1b428 89 //rudderC should be an angle -45 to 45
bclaus 7:e8a77af1b428 90 //map this to 0.00135 to 0.00175
bclaus 7:e8a77af1b428 91 rTime=0.0014+0.003*((rudderC+45.0)/90.0);
bclaus 7:e8a77af1b428 92 //thrusterC should be an percentage from -100 to 100
bclaus 7:e8a77af1b428 93 //map this to 0.0011 to 0.0019
bclaus 7:e8a77af1b428 94 //0.0019 is full reverse
bclaus 7:e8a77af1b428 95 //0.0011 is full ahead
bclaus 7:e8a77af1b428 96 tTime=0.0019 - 0.008*((thrusterC+100.0)/200.0);
bclaus 6:2d7e4561625c 97
bclaus 7:e8a77af1b428 98 radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);
bclaus 7:e8a77af1b428 99 if(rTime < 0.001 || rTime > 0.002)rTime=0.00155;
bclaus 7:e8a77af1b428 100 if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
bclaus 7:e8a77af1b428 101 rudderOut.pulsewidth(rTime);
bclaus 7:e8a77af1b428 102
bclaus 7:e8a77af1b428 103 thrusterOut.pulsewidth(tTime);
bclaus 7:e8a77af1b428 104 radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);
bclaus 6:2d7e4561625c 105 }
bclaus 7:e8a77af1b428 106 //make sure this doesn't run too fast
bclaus 7:e8a77af1b428 107 if((t.read()-tLoopi)<0.1){
bclaus 7:e8a77af1b428 108 wait(0.1-(t.read()-tLoopi));
bclaus 4:7fb44cbc97a3 109 }
bclaus 4:7fb44cbc97a3 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 7:e8a77af1b428 172 }else{
bclaus 7:e8a77af1b428 173 //do nothing, this will eat all the garbage chars...
bclaus 4:7fb44cbc97a3 174 }
bclaus 5:ce6688d37d12 175
bclaus 7:e8a77af1b428 176
bclaus 7:e8a77af1b428 177 }
bclaus 7:e8a77af1b428 178
bclaus 7:e8a77af1b428 179
bclaus 4:7fb44cbc97a3 180
bclaus 4:7fb44cbc97a3 181 }
bclaus 4:7fb44cbc97a3 182
bclaus 4:7fb44cbc97a3 183 void radioFlush(void) {
bclaus 4:7fb44cbc97a3 184 while (radio.readable()) {
bclaus 4:7fb44cbc97a3 185 radio.getc();
bclaus 4:7fb44cbc97a3 186 }
bclaus 4:7fb44cbc97a3 187 return;
bclaus 4:7fb44cbc97a3 188 }