Revision 4:7fb44cbc97a3, committed 2013-09-05

Comitter:

bclaus

Date:

Thu Sep 05 14:36:25 2013 +0000

Parent:

3:c348ce854b4c

Child:

5:ce6688d37d12

Commit message:

added command structure for manual control and command control;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PwmIn.lib	Thu Sep 05 14:36:25 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/PwmIn/#6d68eb9b6bbb

--- a/main.cpp	Wed Jul 31 15:37:16 2013 +0000
+++ b/main.cpp	Thu Sep 05 14:36:25 2013 +0000
@@ -2,6 +2,7 @@
 #include "MMA8451Q.h"
 #include "TinyGPS.h"
 #include "HMC5883L.h"
+#include "PwmIn.h"
  
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
@@ -13,14 +14,28 @@
 Timer t;
 TinyGPS gps;
 Serial gpsSerial(PTD3,PTD2);
-
+PwmIn rudderIn(PTD0);
+PwmIn thrusterIn(PTD5);
+PwmOut rudderOut(PTA4);
+PwmOut thrusterOut(PTA5);
 
 void gpsSerialRecvInterrupt (void);
+void radioSerialRecvInterrupt (void);
+void radioFlush(void);
+
+bool manual = true;
+int commandLength = 2;
+//radio commands
+enum{ thruster='a',
+    rudder,
+    combo,
+    control};
+    
+float rudderC = 0.0015, thrusterC = 0.0015;
 
 int main(void) {
 
     
-    
     pc.baud(115200);
     radio.baud(57600);
     int16_t dCompass[3];
@@ -28,8 +43,12 @@
     compass.init();
     pc.printf("inited\r\n");
     gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq);    // Recv interrupt handler
+    radio.attach (&radioSerialRecvInterrupt, radio.RxIrq);    // Recv interrupt handler
     gps.reset_ready();
- 
+    rudderOut.period(0.02);
+    thrusterOut.period(0.02);
+    rudderOut.pulsewidth(0.0015);
+    thrusterOut.pulsewidth(0.0015);
 
 
  
@@ -47,7 +66,22 @@
         compass.getXYZ(dCompass);
         pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
         radio.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
-        wait(0.5);
+        if(manual){
+            wait(0.2);
+            rudderOut.pulsewidth(rudderIn.pulsewidth());
+            thrusterOut.pulsewidth(thrusterIn.pulsewidth());
+            }
+        else{
+            wait(0.2);
+            //rudderC should be an angle -45 to 45
+            //map this to 0.0013 to 0.0017
+            if(rudderC > -45 && rudderC < 45){
+                rudderOut.pulsewidth(0.0013+0.004*((rudderC+45)/90));
+            }
+            //thrusterC should be an percentage from 0 to 100
+            //map this to 0.0013 to 0.0017
+            thrusterOut.pulsewidth(0.0013 + 0.004*(thrusterC/100));
+        }
     }
 }
 
@@ -64,4 +98,44 @@
     //}
 
 
-}
\ No newline at end of file
+}
+void radioSerialRecvInterrupt (void)
+{
+    
+    int firstByte, secondByte;
+    //get command
+    firstByte = radio.getc();  
+    if(firstByte == 'c'){
+        manual = false;
+        secondByte = radio.getc();  
+        switch(secondByte){
+            case thruster:
+                //example command 'ca 50'
+                radio.scanf(" %f",thrusterC);
+                break;
+            case rudder:
+                //example command 'cb -20'
+                radio.scanf(" %f",rudderC);
+                break;
+            case combo:
+                //example command 'cc -20,50'
+                radio.scanf(" %f,%f",rudderC,thrusterC);
+                break;
+            case control:
+                //example command 'cd'
+                manual = true;
+                break;
+            default:
+            //shouldn't be here flush stream
+                radioFlush();
+        }
+    }
+
+}
+
+void radioFlush(void) { 
+    while (radio.readable()) { 
+        radio.getc(); 
+    } 
+    return; 
+}