Revision 7:e8a77af1b428, committed 2013-09-12

Comitter:

bclaus

Date:

Thu Sep 12 13:57:36 2013 +0000

Parent:

6:2d7e4561625c

Commit message:

Revision 0. This is the first one that really works;

Changed in this revision

diff -r 2d7e4561625c -r e8a77af1b428 main.cpp
--- a/main.cpp	Thu Sep 05 18:55:20 2013 +0000
+++ b/main.cpp	Thu Sep 12 13:57:36 2013 +0000
@@ -14,10 +14,10 @@
 Timer t;
 TinyGPS gps;
 Serial gpsSerial(PTD3,PTD2);
-PwmIn rudderIn(PTD5);
-PwmIn thrusterIn(PTD0);
-PwmOut rudderOut(PTA4);
-PwmOut thrusterOut(PTA5);
+PwmIn thrusterIn(PTD5);
+PwmIn rudderIn(PTD0);
+PwmOut rudderOut(PTA5);
+PwmOut thrusterOut(PTA4);
 
 void gpsSerialRecvInterrupt (void);
 void radioSerialRecvInterrupt (void);
@@ -31,11 +31,11 @@
     combo,
     control};
     
-float rudderC = 0.0015, thrusterC = 0.0015;
+int rudderC = 0, thrusterC = 0;
 
 int main(void) {
-
-    int firstByte, secondByte;
+    float tLoop=0,tLoopi=0;
+    float rTime=0.00155,tTime=0.00165;
     pc.baud(115200);
     radio.baud(57600);
     
@@ -46,79 +46,67 @@
     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);
-    rudderOut.pulsewidth(0.0015);
-    thrusterOut.pulsewidth(0.0015);
+    rudderOut.pulsewidth(0.0016);
+    thrusterOut.pulsewidth(0.00165);
 
 
- 
+    t.start();
     while (true) {
-        
+        tLoop =t.read();
         if(gps.gga_ready()){
            
-            pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
-            radio.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
+            //pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
+            radio.printf("GPS: %.4f, %.8f, %.8f, %.4f, %.4f\r\n", t.read(),gps.f_lat(), gps.f_lon(),gps.f_course(),gps.f_speed_knots());
         }
 
         acc.getAccAllAxis(dAcc);
-        pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
-        radio.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
+        //pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
+        radio.printf("ACC: %.4f, %.4f, %.4f, %.4f\r\n", t.read(),dAcc[0], dAcc[1], dAcc[2]);
         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]);
+        //pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
+        radio.printf("MAG: %.4f, %4d, %4d, %4d\r\n", t.read(),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();
+        while(t.read() - tLoop < 0.5){
+            tLoopi=t.read();
+            if(manual){               
+                //low pass filter the rudder time to stop the jitter
+                rTime=rTime + 0.8*(rudderIn.pulsewidth()-rTime);
+                //if outside limits center the rudder
+                if(rTime < 0.001 || rTime > 0.002)rTime=0.00155; 
+                tTime =tTime +0.2*(thrusterIn.pulsewidth()-tTime);
+                //if outside limits thruster to zero
+                if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
+                
+                rudderOut.pulsewidth(rTime);
+                thrusterOut.pulsewidth(tTime);
+                radio.printf("VEH:%.4f, %.6f, %.6f\r\n", t.read(),tTime, rTime);
                 }
-            }
             else{
-                //we shouldnt be here do nothing
+                //rudderC should be an angle -45 to 45
+                //map this to 0.00135 to 0.00175
+                rTime=0.0014+0.003*((rudderC+45.0)/90.0);     
+                //thrusterC should be an percentage from -100 to 100
+                //map this to 0.0011 to 0.0019
+                //0.0019 is full reverse
+                //0.0011 is full ahead              
+                tTime=0.0019 - 0.008*((thrusterC+100.0)/200.0);  
                 
+                radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);         
+                if(rTime < 0.001 || rTime > 0.002)rTime=0.00155;   
+                if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
+                rudderOut.pulsewidth(rTime);
+                
+                thrusterOut.pulsewidth(tTime);            
+                radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);
             }
-        }
-        
-        if(manual){
-            wait(0.5);
-            rudderOut.pulsewidth(rudderIn.pulsewidth());
-            thrusterOut.pulsewidth(thrusterIn.pulsewidth());
+            //make sure this doesn't run too fast
+            if((t.read()-tLoopi)<0.1){
+                wait(0.1-(t.read()-tLoopi));
             }
-        else{
-            wait(0.5);
-            //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));
         }
     }
 }
@@ -140,39 +128,55 @@
 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();
+    int firstByte=0;
+    int command='a';
+
+    if(radio.readable())firstByte=radio.getc();
+ 
+     if(command=='t'){
+        
+        thrusterC = firstByte-128;
+        //limit thrusterC
+        if(thrusterC>100){thrusterC=100;}
+        if(thrusterC<-100){thrusterC=-100;}
+        command=0;
+    }
+    else if(command=='r'){
+        
+        rudderC=firstByte-128;
+        //limit rudderC
+        if(rudderC > 45){rudderC=45;}
+        if(rudderC<-45){rudderC=-45;}
+        command=0;
+    }
+ 
+   if(manual==true){
+        //get command
+        if(firstByte=='a'){
+             manual=false;
+             command='a';
+        } 
+   }else{
+        //check if override
+        if(firstByte=='b'){
+             manual=true;
+             command='b';
+        }else if(firstByte=='r'){
+        //take in 0-255 as -45 to 45 where 128 is 0
+
+            command='r';
+            
+        }else if(firstByte=='t'){ 
+
+            command=='t';
+        }else{
+            //do nothing, this will eat all the garbage chars...
         }
-    }
-    else{
-        //we shouldnt be here do nothing
         
-    }
+   
+   }
+     
+   
 
 }