brian claus / Mbed 2 deprecated tugboat

tugboat project

Dependencies:   TinyGPS HMC5883L MMA8451Q mbed PwmIn

main.cpp@5:ce6688d37d12, 2013-09-05 (annotated)

Committer:
bclaus
Date:
Thu Sep 05 18:04:18 2013 +0000
Revision:
5:ce6688d37d12
Parent:
4:7fb44cbc97a3
Child:
6:2d7e4561625c
temp commit;

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 4:7fb44cbc97a3 17 PwmIn rudderIn(PTD0);
bclaus 4:7fb44cbc97a3 18 PwmIn thrusterIn(PTD5);
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 0:ee158c8007b4 38
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 4:7fb44cbc97a3 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 4:7fb44cbc97a3 72 if(manual){
bclaus 4:7fb44cbc97a3 73 wait(0.2);
bclaus 4:7fb44cbc97a3 74 rudderOut.pulsewidth(rudderIn.pulsewidth());
bclaus 4:7fb44cbc97a3 75 thrusterOut.pulsewidth(thrusterIn.pulsewidth());
bclaus 4:7fb44cbc97a3 76 }
bclaus 4:7fb44cbc97a3 77 else{
bclaus 4:7fb44cbc97a3 78 wait(0.2);
bclaus 4:7fb44cbc97a3 79 //rudderC should be an angle -45 to 45
bclaus 4:7fb44cbc97a3 80 //map this to 0.0013 to 0.0017
bclaus 4:7fb44cbc97a3 81 if(rudderC > -45 && rudderC < 45){
bclaus 4:7fb44cbc97a3 82 rudderOut.pulsewidth(0.0013+0.004*((rudderC+45)/90));
bclaus 4:7fb44cbc97a3 83 }
bclaus 4:7fb44cbc97a3 84 //thrusterC should be an percentage from 0 to 100
bclaus 4:7fb44cbc97a3 85 //map this to 0.0013 to 0.0017
bclaus 4:7fb44cbc97a3 86 thrusterOut.pulsewidth(0.0013 + 0.004*(thrusterC/100));
bclaus 4:7fb44cbc97a3 87 }
bclaus 0:ee158c8007b4 88 }
bclaus 2:db76adcdf799 89 }
bclaus 2:db76adcdf799 90
bclaus 2:db76adcdf799 91 //*****************************************************************************
bclaus 2:db76adcdf799 92 // serial receive interrupt handler
bclaus 2:db76adcdf799 93 //*****************************************************************************
bclaus 2:db76adcdf799 94
bclaus 2:db76adcdf799 95 void gpsSerialRecvInterrupt (void)
bclaus 2:db76adcdf799 96 {
bclaus 2:db76adcdf799 97
bclaus 2:db76adcdf799 98
bclaus 2:db76adcdf799 99 // while(gpsSerial.readable()){
bclaus 2:db76adcdf799 100 gps.encode(gpsSerial.getc());
bclaus 2:db76adcdf799 101 //}
bclaus 2:db76adcdf799 102
bclaus 2:db76adcdf799 103
bclaus 4:7fb44cbc97a3 104 }
bclaus 4:7fb44cbc97a3 105 void radioSerialRecvInterrupt (void)
bclaus 4:7fb44cbc97a3 106 {
bclaus 4:7fb44cbc97a3 107
bclaus 4:7fb44cbc97a3 108 int firstByte, secondByte;
bclaus 4:7fb44cbc97a3 109 //get command
bclaus 4:7fb44cbc97a3 110 firstByte = radio.getc();
bclaus 4:7fb44cbc97a3 111 if(firstByte == 'c'){
bclaus 4:7fb44cbc97a3 112 manual = false;
bclaus 4:7fb44cbc97a3 113 secondByte = radio.getc();
bclaus 4:7fb44cbc97a3 114 switch(secondByte){
bclaus 4:7fb44cbc97a3 115 case thruster:
bclaus 4:7fb44cbc97a3 116 //example command 'ca 50'
bclaus 4:7fb44cbc97a3 117 radio.scanf(" %f",thrusterC);
bclaus 4:7fb44cbc97a3 118 break;
bclaus 4:7fb44cbc97a3 119 case rudder:
bclaus 4:7fb44cbc97a3 120 //example command 'cb -20'
bclaus 4:7fb44cbc97a3 121 radio.scanf(" %f",rudderC);
bclaus 4:7fb44cbc97a3 122 break;
bclaus 4:7fb44cbc97a3 123 case combo:
bclaus 4:7fb44cbc97a3 124 //example command 'cc -20,50'
bclaus 4:7fb44cbc97a3 125 radio.scanf(" %f,%f",rudderC,thrusterC);
bclaus 4:7fb44cbc97a3 126 break;
bclaus 4:7fb44cbc97a3 127 case control:
bclaus 4:7fb44cbc97a3 128 //example command 'cd'
bclaus 4:7fb44cbc97a3 129 manual = true;
bclaus 4:7fb44cbc97a3 130 break;
bclaus 4:7fb44cbc97a3 131 default:
bclaus 4:7fb44cbc97a3 132 //shouldn't be here flush stream
bclaus 4:7fb44cbc97a3 133 radioFlush();
bclaus 4:7fb44cbc97a3 134 }
bclaus 4:7fb44cbc97a3 135 }
bclaus 5:ce6688d37d12 136 else{
bclaus 5:ce6688d37d12 137 //we shouldnt be here do nothing
bclaus 5:ce6688d37d12 138
bclaus 5:ce6688d37d12 139 }
bclaus 4:7fb44cbc97a3 140
bclaus 4:7fb44cbc97a3 141 }
bclaus 4:7fb44cbc97a3 142
bclaus 4:7fb44cbc97a3 143 void radioFlush(void) {
bclaus 4:7fb44cbc97a3 144 while (radio.readable()) {
bclaus 4:7fb44cbc97a3 145 radio.getc();
bclaus 4:7fb44cbc97a3 146 }
bclaus 4:7fb44cbc97a3 147 return;
bclaus 4:7fb44cbc97a3 148 }