Diff: main.cpp

Revision:

6:2d7e4561625c

Parent:

5:ce6688d37d12

Child:

7:e8a77af1b428

diff -r ce6688d37d12 -r 2d7e4561625c main.cpp
--- a/main.cpp	Thu Sep 05 18:04:18 2013 +0000
+++ b/main.cpp	Thu Sep 05 18:55:20 2013 +0000
@@ -14,8 +14,8 @@
 Timer t;
 TinyGPS gps;
 Serial gpsSerial(PTD3,PTD2);
-PwmIn rudderIn(PTD0);
-PwmIn thrusterIn(PTD5);
+PwmIn rudderIn(PTD5);
+PwmIn thrusterIn(PTD0);
 PwmOut rudderOut(PTA4);
 PwmOut thrusterOut(PTA5);
 
@@ -35,7 +35,7 @@
 
 int main(void) {
 
-    
+    int firstByte, secondByte;
     pc.baud(115200);
     radio.baud(57600);
     
@@ -46,7 +46,7 @@
     compass.init();
     pc.printf("inited\r\n");
     gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq);    // Recv interrupt handler
-    radio.attach (&radioSerialRecvInterrupt, radio.RxIrq);    // Recv interrupt handler
+    //radio.attach (&radioSerialRecvInterrupt, radio.RxIrq);    // Recv interrupt handler
     gps.reset_ready();
     rudderOut.period(0.02);
     thrusterOut.period(0.02);
@@ -69,13 +69,48 @@
         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]);
+        
+        if(radio.readable()){
+            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();
+                }
+            }
+            else{
+                //we shouldnt be here do nothing
+                
+            }
+        }
+        
         if(manual){
-            wait(0.2);
+            wait(0.5);
             rudderOut.pulsewidth(rudderIn.pulsewidth());
             thrusterOut.pulsewidth(thrusterIn.pulsewidth());
             }
         else{
-            wait(0.2);
+            wait(0.5);
             //rudderC should be an angle -45 to 45
             //map this to 0.0013 to 0.0017
             if(rudderC > -45 && rudderC < 45){
@@ -107,6 +142,7 @@
     
     int firstByte, secondByte;
     //get command
+    
     firstByte = radio.getc();  
     if(firstByte == 'c'){
         manual = false;