Diff: main.cpp

Revision:

27:9ac59b261d87

Parent:

26:dc00998140af

Child:

28:5905886c76ee

--- a/main.cpp	Fri Jan 16 17:26:07 2015 +0000
+++ b/main.cpp	Mon Jan 19 01:35:34 2015 +0000
@@ -9,8 +9,12 @@
 #include "Line.h"
 #include "stringUtils.h"
 
+char *version="FreePilot V2.1 Jan 16, 2015\r\n";
+long lastsend_version=0;
+Timer vTimer; //this timer is int based! Max is 30 minutes
+
 // Connect the TX of the GPS module to p10 RX input
-MODSERIAL b(p9, p10);
+MODSERIAL b(p9, p10); //GPS
 MODSERIAL pc(USBTX, USBRX);
 MODSERIAL bluetooth(p13, p14);
 int checksumm;
@@ -39,13 +43,28 @@
 Ticker  angle_print;
 Ticker  debug_leds;
 
-PinDetect  motor_switch(p24); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
-DigitalOut enable_motor(p21);
+//Motor
+PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
+DigitalOut enable_motor(p7);
 DigitalOut led1(p11);
 DigitalOut led2(p12);
-PwmOut pwm1(p22);
-PwmOut pwm2(p23);
+PwmOut pwm1(p21);
+PwmOut pwm2(p22);
 
+//equipment switches
+PinDetect  boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
+PinDetect  boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
+PinDetect  boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
+PinDetect  boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
+//DigitalIn boom1(p20);
+//DigitalIn boom2(p19);
+//DigitalIn boom3(p18);
+//DigitalIn boom4(p17);
+char boom18; //1 byte
+char lastboom18; //1 byte
+char boomstate[8]={'$','F','B','S',0,13,10,0 };
+
+//gyro
 IMUfilter imuFilter(FILTER_RATE, 0.3);
 ADXL345_I2C accelerometer(p28, p27);
 ITG3200 gyroscope(p28,p27);
@@ -346,7 +365,7 @@
 
 void activate_antenna()
 {
-b.baud(gps_baud);
+    b.baud(gps_baud);
     antenna_active = 1;
 }
 
@@ -549,7 +568,7 @@
                 pc.puts(output);
     char *token;
     int  token_counter = 0;
-    char *cs   = (char *)NULL;
+   // char *cs   = (char *)NULL;
     token = strtok(cs_string, "*");
     while (token) 
     {
@@ -661,8 +680,8 @@
         double filtering = 111.111 / 2.0 + 111.111 * cos(decimal_latitude)/2.0;
         distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end);
        
-        sprintf(output,"$DIST_TO_LINE: % .12f\r\n\0",distance_to_line * filtering);
-        pc.puts(output);
+     //   sprintf(output,"$DIST_TO_LINE: % .12f\r\n\0",distance_to_line * filtering);
+     //   pc.puts(output);
 
 }
 
@@ -783,7 +802,7 @@
             pc.puts(output);
     }
 }
-
+//sets pwm1 and pwm2 and enable_motor
 void process_ASTEER(char* asteer)
 {
     char *token;
@@ -893,19 +912,33 @@
 {
     if (!strncmp(pc_string, "$ASTEER", 7)) 
     {
+        //sets pwm1 and pwm2 and enable_motor
         process_ASTEER(pc_string);
     } 
-    else if (!strncmp(pc_string, "$GPS-BAUD",9)) 
+    else if (!strncmp(pc_string, "$FGPS-BAUD",10)) 
     {
         process_GPSBAUD(pc_string);
-        sprintf(output,"%s\r\n",pc_string);
-        bluetooth.puts(output);
+        sprintf(output,"%s %d\r\n",pc_string,gps_baud);
+        pc.puts(output);
+        
     } 
     else if (!strncmp(pc_string, "$FGPS,",6)) 
     {
-       process_initstring(pc_string);
-       sprintf(output,"%s\r\n",pc_string);
-       bluetooth.puts(output);
+       
+       //process_initstring(pc_string);
+       int i=5;
+       char c=pc_string[i];
+       while (c!=0)
+       {
+          i++;
+          if (i>255) break; //protect msg buffer!
+          c=pc_string[i];
+          b.putc(c); 
+          pc.putc(c);   
+       }
+      // sprintf(output,"%s\r\n",pc_string);
+      // pc.puts(output);
+      // b.puts(output);
     } 
     else if (!strncmp(pc_string, "$PRINT_VTG=0",12)) 
     {
@@ -1039,40 +1072,89 @@
     }
 }*/
 
-void getline2()
+
+
+
+int i2 = 0;
+bool end2 = false;
+bool start2 = false;
+
+bool getline2()
 {
-    for (int i = 0; i<126; i++) 
-    {
-        msg2[i] = bluetooth.getc();
-        if (msg2[i] == '\n') 
+    int gotstring=false;
+    while (1)
+    {    
+        if( !bluetooth.readable() )
+        {
+            break;
+        }
+        char c = bluetooth.getc();
+        if (c == 36 ){start2=true;end2 = false; i2 = 0;}
+        if (c == 10) {end2=true; start2 = false;}
+        if (start2)
         {
-            msg2[i+1] = 0;
-            dont = 0;
-            return;
+            msg2[i2]=c;
+            i2++;
+            if (i2>255) break; //protect msg buffer!
+        }
+        if (end2)
+        {
+            msg2[i2]=c;   
+            msg2[i2+1] = 0;
+            start2 = false;
+            gotstring = true;
+            end2=false;
+            i2=0;
+            break;
         }
     }
+    return gotstring;
 }
 
-void getline()
+
+int i=0;
+bool start=false;
+bool end=false;
+    
+bool getline(bool forward)
 {
-    while (b.getc() != '$');
-    msg[0] = '$';  // wait for the start of a line
-    for (int i=1; i<256; i++) 
-    {
-        msg[i] = b.getc();
-        if (msg[i] == '\n') 
+    while (1)
+    {    
+        if( !b.readable() )
+        {
+            break;
+        }
+        char c = b.getc();
+        if (forward) //simply forward all to Bluetooth
         {
+           bluetooth.putc(c);    
+        }
+        if (c == 36 ){start=true;end = false; i = 0;}
+        if (c == 10) {end=true; start = false;}
+        if (start)
+        {
+            msg[i]=c;
+            i++;
+            if (i>255) break; //protect msg buffer!
+        }
+        if (end)
+        {
+            msg[i]=c;   
             msg[i+1] = 0;
-            return;
+            start = false;
+            end = true;
+            break;
         }
     }
+    return end;
 }
 
+
 int sample()
 {
     while (1) 
     {
-        getline();
+        getline(false);
         return 1;
     }
 }
@@ -1088,16 +1170,71 @@
 
 void keyPressedHeld( void )
 {
-    motor_enable_state = "$ENABLE=1\r\n";
+//    motor_enable_state = "$ENABLE=1\r\n";
     motor_enable_tosend = 1;
+  //  led1=1;
 }
 
 void keyReleasedHeld( void )
 {
-    motor_enable_state = "$ENABLE=0\r\n";
-    motor_enable_tosend = 1;
+ //   motor_enable_state = "$ENABLE=0\r\n";
+    motor_enable_tosend = 0;
+//    led1=0;
+}
+
+void boom1PressedHeld( void )
+{
+  
+   // led1=1;
+ boom18=boom18 & 0xFE;
+   //  sprintf(output,"Boom1 Pressed %d",boom18);     
+   //  pc.puts(output);
+}
+
+void boom1ReleasedHeld( void )
+{
+  led1=0;  
+  boom18=boom18 | 0x01;
+
+}
+
+void boom2PressedHeld( void )
+{  
+   boom18=boom18 & 0xFD;
+  
 }
 
+void boom2ReleasedHeld( void )
+{
+ 
+   boom18=boom18 | 0x02;
+}
+void boom3PressedHeld( void )
+{
+    
+     boom18=boom18 & 0xFB;
+}
+
+void boom3ReleasedHeld( void )
+{
+  
+  boom18=boom18 | 0x04;
+}
+
+void boom4PressedHeld( void )
+{
+    
+   boom18=boom18 & 0xF7;
+}
+
+void boom4ReleasedHeld( void )
+{
+    
+  boom18=boom18 | 0x08;
+}
+
+
+
 void toprint()
 {
     angle_send = 1;
@@ -1122,9 +1259,42 @@
 
 int main()
 {
-    bluetooth.baud(38400);
+    int x=0;
+    bluetooth.baud(115200);
     b.baud(38400);
 pc.baud(38400);
+
+//JH prepare and send version info
+    vTimer.start();
+    vTimer.reset();
+
+    pc.puts(version);
+    bluetooth.puts(version);
+    lastsend_version=vTimer.read_ms();
+    
+  
+    boom1.attach_asserted_held( &boom1PressedHeld );
+    boom1.attach_deasserted_held( &boom1ReleasedHeld );
+    boom1.setSampleFrequency(); //default = 20 ms
+    boom1.setSamplesTillAssert(5);
+    boom1.setSamplesTillHeld(5);
+    boom2.attach_asserted_held( &boom2PressedHeld );
+    boom2.attach_deasserted_held( &boom2ReleasedHeld );
+    boom2.setSamplesTillAssert(5);
+    boom2.setSamplesTillHeld(5);
+    boom2.setSampleFrequency();
+    boom3.attach_asserted_held( &boom3PressedHeld );
+    boom3.attach_deasserted_held( &boom3ReleasedHeld );
+    boom3.setSamplesTillAssert(5);
+    boom3.setSamplesTillHeld(5);
+    boom3.setSampleFrequency();
+    boom4.attach_asserted_held( &boom4PressedHeld );
+    boom4.attach_deasserted_held( &boom4ReleasedHeld );
+    boom4.setSamplesTillAssert(5);
+    boom4.setSamplesTillHeld(5);
+    boom4.setSampleFrequency();
+       
+       
     motor_switch.setSampleFrequency(10000);
     motor_switch.attach_asserted_held( &keyPressedHeld );
     motor_switch.attach_deasserted_held( &keyReleasedHeld );
@@ -1140,47 +1310,79 @@
     activate_antenna();
     while(1) 
     {
-        if ( antenna_active == 1 ) 
+    
+        //JH send version information every 10 seconds to keep Bluetooth alive
+        if ((vTimer.read_ms()-lastsend_version)>10000) 
         {
-            if ( b.readable()) 
+       
+            pc.puts(version);
+            bluetooth.puts(version);
+            vTimer.reset();
+            lastsend_version=vTimer.read_ms();
+          //  sprintf(output,"GPS Baudrate %d \r\n",gps_baud);     
+          //  pc.puts(output);
+          //  sprintf(output,"Boom18 %d \r\n",boom18);     
+          //  pc.puts(output);
+        } 
+        
+        if ( antenna_active == 1 && b.readable()) 
+        {
+            if (getline(true)) 
             {
-                if (sample()) 
-                {
-                    checksumm = getCheckSum(msg);
-                    if ( pc.writeable()) 
-                    {
-                        sprintf(output,"%s\r\n\0",msg);
-                        pc.puts(output);
-                    }
-                    gps_analyse(msg);
-                }
+               // checksumm = getCheckSum(msg);
+               // gps_analyse(msg);
             }
         }
 
+
         if ( bluetooth.readable()) 
         {
-            if (samplepc()) 
-            {
-                trim(msg2,"\r");
-                trim(msg2,"\n");
-                trim(msg2,"\0");
+           if (getline2()) 
+           {
+               x++;
+              //  trim(msg2,"\r");
+             //   trim(msg2,"\n");
+              //  trim(msg2,"\0");
                 trim(msg2," ");
-                sprintf(output,"%s\0",msg2);
+                sprintf(output,"%d %s",x,msg2);
                 pc.puts(output);
+             //pc.puts(output);
                 pc_analyse(msg2);
+           
             }
         }
-        if ( motor_enable_tosend == 1 && reading == 0 ) 
+
+        if ( motor_enable_tosend == 1 ) //&& reading == 0 ) 
         {
+            sprintf(output,"BT %f %f",pwm1_speed,pwm2_speed);     
+            pc.puts(output);
+                
             bluetooth.puts(motor_enable_state);
             motor_enable_tosend = 0;
         }
-     /*  if ( print_euler == 1 && angle_send == 1 && reading == 0) 
+
+        //bounces too much
+        //if (boom1) boom18=boom18 & 0xFE;
+        //else boom18=boom18 | 0x01;
+  
+        if (boom18!=lastboom18)
+        {
+           //  sprintf(output,"Change boom18= %d\r\n",boom18);
+           //  pc.puts(output);
+          
+             boomstate[4]=boom18 | 0x80; //
+             bluetooth.puts(boomstate);
+             pc.puts(boomstate);
+             lastboom18=boom18;
+        }
+
+   /*     if ( print_euler == 1 && angle_send == 1 ) //&& reading == 0) 
         {
             sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw()));
             pc.puts(output);
            bluetooth.puts(output);
             angle_send = 0;
-        }*/
+        }
+*/
     }
 }
\ No newline at end of file