brian claus / Mbed 2 deprecated tugboat2

tugboat project

Dependencies:   HMC5883L MMA8451Q PwmIn TinyGPS2 mbed

Fork of tugboat by brian claus

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?

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 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 }