brian claus / Mbed 2 deprecated tugboat

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?

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