brian claus / Mbed 2 deprecated tugboat2

tugboat project

Dependencies:   HMC5883L MMA8451Q PwmIn TinyGPS2 mbed

Fork of tugboat by brian claus

main.cpp@4:7fb44cbc97a3, 2013-09-05 (annotated)

Committer:
bclaus
Date:
Thu Sep 05 14:36:25 2013 +0000
Revision:
4:7fb44cbc97a3
Parent:
2:db76adcdf799
Child:
5:ce6688d37d12
added command structure for manual control and command control;

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 0:ee158c8007b4 41 int16_t dCompass[3];
bclaus 0:ee158c8007b4 42 float dAcc[3];
bclaus 0:ee158c8007b4 43 compass.init();
bclaus 2:db76adcdf799 44 pc.printf("inited\r\n");
bclaus 2:db76adcdf799 45 gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq); // Recv interrupt handler
bclaus 4:7fb44cbc97a3 46 radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler
bclaus 2:db76adcdf799 47 gps.reset_ready();
bclaus 4:7fb44cbc97a3 48 rudderOut.period(0.02);
bclaus 4:7fb44cbc97a3 49 thrusterOut.period(0.02);
bclaus 4:7fb44cbc97a3 50 rudderOut.pulsewidth(0.0015);
bclaus 4:7fb44cbc97a3 51 thrusterOut.pulsewidth(0.0015);
bclaus 0:ee158c8007b4 52
bclaus 0:ee158c8007b4 53
bclaus 0:ee158c8007b4 54
bclaus 0:ee158c8007b4 55 while (true) {
bclaus 0:ee158c8007b4 56
bclaus 2:db76adcdf799 57 if(gps.gga_ready()){
bclaus 2:db76adcdf799 58
bclaus 2:db76adcdf799 59 pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
bclaus 2:db76adcdf799 60 radio.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
bclaus 0:ee158c8007b4 61 }
bclaus 0:ee158c8007b4 62
bclaus 0:ee158c8007b4 63 acc.getAccAllAxis(dAcc);
bclaus 0:ee158c8007b4 64 pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
bclaus 0:ee158c8007b4 65 radio.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
bclaus 0:ee158c8007b4 66 compass.getXYZ(dCompass);
bclaus 0:ee158c8007b4 67 pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
bclaus 0:ee158c8007b4 68 radio.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
bclaus 4:7fb44cbc97a3 69 if(manual){
bclaus 4:7fb44cbc97a3 70 wait(0.2);
bclaus 4:7fb44cbc97a3 71 rudderOut.pulsewidth(rudderIn.pulsewidth());
bclaus 4:7fb44cbc97a3 72 thrusterOut.pulsewidth(thrusterIn.pulsewidth());
bclaus 4:7fb44cbc97a3 73 }
bclaus 4:7fb44cbc97a3 74 else{
bclaus 4:7fb44cbc97a3 75 wait(0.2);
bclaus 4:7fb44cbc97a3 76 //rudderC should be an angle -45 to 45
bclaus 4:7fb44cbc97a3 77 //map this to 0.0013 to 0.0017
bclaus 4:7fb44cbc97a3 78 if(rudderC > -45 && rudderC < 45){
bclaus 4:7fb44cbc97a3 79 rudderOut.pulsewidth(0.0013+0.004*((rudderC+45)/90));
bclaus 4:7fb44cbc97a3 80 }
bclaus 4:7fb44cbc97a3 81 //thrusterC should be an percentage from 0 to 100
bclaus 4:7fb44cbc97a3 82 //map this to 0.0013 to 0.0017
bclaus 4:7fb44cbc97a3 83 thrusterOut.pulsewidth(0.0013 + 0.004*(thrusterC/100));
bclaus 4:7fb44cbc97a3 84 }
bclaus 0:ee158c8007b4 85 }
bclaus 2:db76adcdf799 86 }
bclaus 2:db76adcdf799 87
bclaus 2:db76adcdf799 88 //*****************************************************************************
bclaus 2:db76adcdf799 89 // serial receive interrupt handler
bclaus 2:db76adcdf799 90 //*****************************************************************************
bclaus 2:db76adcdf799 91
bclaus 2:db76adcdf799 92 void gpsSerialRecvInterrupt (void)
bclaus 2:db76adcdf799 93 {
bclaus 2:db76adcdf799 94
bclaus 2:db76adcdf799 95
bclaus 2:db76adcdf799 96 // while(gpsSerial.readable()){
bclaus 2:db76adcdf799 97 gps.encode(gpsSerial.getc());
bclaus 2:db76adcdf799 98 //}
bclaus 2:db76adcdf799 99
bclaus 2:db76adcdf799 100
bclaus 4:7fb44cbc97a3 101 }
bclaus 4:7fb44cbc97a3 102 void radioSerialRecvInterrupt (void)
bclaus 4:7fb44cbc97a3 103 {
bclaus 4:7fb44cbc97a3 104
bclaus 4:7fb44cbc97a3 105 int firstByte, secondByte;
bclaus 4:7fb44cbc97a3 106 //get command
bclaus 4:7fb44cbc97a3 107 firstByte = radio.getc();
bclaus 4:7fb44cbc97a3 108 if(firstByte == 'c'){
bclaus 4:7fb44cbc97a3 109 manual = false;
bclaus 4:7fb44cbc97a3 110 secondByte = radio.getc();
bclaus 4:7fb44cbc97a3 111 switch(secondByte){
bclaus 4:7fb44cbc97a3 112 case thruster:
bclaus 4:7fb44cbc97a3 113 //example command 'ca 50'
bclaus 4:7fb44cbc97a3 114 radio.scanf(" %f",thrusterC);
bclaus 4:7fb44cbc97a3 115 break;
bclaus 4:7fb44cbc97a3 116 case rudder:
bclaus 4:7fb44cbc97a3 117 //example command 'cb -20'
bclaus 4:7fb44cbc97a3 118 radio.scanf(" %f",rudderC);
bclaus 4:7fb44cbc97a3 119 break;
bclaus 4:7fb44cbc97a3 120 case combo:
bclaus 4:7fb44cbc97a3 121 //example command 'cc -20,50'
bclaus 4:7fb44cbc97a3 122 radio.scanf(" %f,%f",rudderC,thrusterC);
bclaus 4:7fb44cbc97a3 123 break;
bclaus 4:7fb44cbc97a3 124 case control:
bclaus 4:7fb44cbc97a3 125 //example command 'cd'
bclaus 4:7fb44cbc97a3 126 manual = true;
bclaus 4:7fb44cbc97a3 127 break;
bclaus 4:7fb44cbc97a3 128 default:
bclaus 4:7fb44cbc97a3 129 //shouldn't be here flush stream
bclaus 4:7fb44cbc97a3 130 radioFlush();
bclaus 4:7fb44cbc97a3 131 }
bclaus 4:7fb44cbc97a3 132 }
bclaus 4:7fb44cbc97a3 133
bclaus 4:7fb44cbc97a3 134 }
bclaus 4:7fb44cbc97a3 135
bclaus 4:7fb44cbc97a3 136 void radioFlush(void) {
bclaus 4:7fb44cbc97a3 137 while (radio.readable()) {
bclaus 4:7fb44cbc97a3 138 radio.getc();
bclaus 4:7fb44cbc97a3 139 }
bclaus 4:7fb44cbc97a3 140 return;
bclaus 4:7fb44cbc97a3 141 }