Diff: main.cpp

Revision:

29:23ccb2a50b6f

Parent:

28:5905886c76ee

Child:

30:3afafa1ef16b

--- a/main.cpp	Wed Jan 21 02:54:26 2015 +0000
+++ b/main.cpp	Thu Jan 22 20:35:42 2015 +0000
@@ -832,10 +832,17 @@
             pwm2_speed = 0;
             enable_motor = 0;
         }
-        sprintf(output,"STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);     
-        pc.puts(output);
-        pwm1 = pwm1_speed;
-        pwm2 = pwm2_speed;
+        if(Authenticated)
+        {
+           pwm1 = pwm1_speed;
+           pwm2 = pwm2_speed;
+        }
+        else
+        {
+            sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);     
+            pc.puts(output);
+            bluetooth.puts(output);
+        }
     }
 }
             
@@ -902,6 +909,12 @@
         //sets pwm1 and pwm2 and enable_motor
         process_ASTEER(pc_string);
     } 
+    else if (!strncmp(pc_string, "$BANY",5)) 
+    {
+        
+        _ID = Config_GetID();
+        Config_Save();
+    }
     else if (!strncmp(pc_string, "$FGPS-BAUD",10)) 
     {
         process_GPSBAUD(pc_string);
@@ -981,9 +994,9 @@
     } 
     else if (!strncmp(pc_string, "$FGPSAB",7)) 
     {
-        sprintf(output,"%s\r\n",pc_string);
+      //  sprintf(output,"FOUND AB %s\r\n",pc_string);
       //  bluetooth.puts(output);
-        pc.puts(output);
+      //  pc.puts(output);
         process_FGPSAB(pc_string);
 
     } 
@@ -1077,8 +1090,12 @@
         }
         char c = bluetooth.getc();
         if (c == 36 ){start2=true;end2 = false; i2 = 0;}
-        if (c == 10) {end2=true; start2 = false;}
-        if (start2)
+        if ((start2) && (c == 10))
+        {
+            end2=true; 
+            start2 = false;
+        }
+        if (start2) 
         {
             msg2[i2]=c;
             i2++;
@@ -1117,7 +1134,11 @@
            bluetooth.putc(c);    
         }
         if (c == 36 ){start=true;end = false; i = 0;}
-        if (c == 10) {end=true; start = false;}
+        if ((start) && (c == 10))
+        {
+            end=true; 
+            start = false;
+        }
         if (start)
         {
             msg[i]=c;
@@ -1126,8 +1147,10 @@
         }
         if (end)
         {
+            
             msg[i]=c;   
             msg[i+1] = 0;
+            i=0;
             start = false;
             end = true;
             break;
@@ -1256,14 +1279,19 @@
     vTimer.reset();
     motTimer.start();
     motTimer.reset();
+    lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
+    motor_enable_state = "$ENABLE,0\r\n";
+     
     btTimer.start();
     btTimer.reset();
     lastgetBT=  btTimer.read_ms();
     
     pc.puts(version);
     bluetooth.puts(version);
-    lastsend_version=vTimer.read_ms();
+    lastsend_version=vTimer.read_ms()-18000;
     
+     _ID = Config_GetID();
+     Config_Save();
   
     boom1.attach_asserted_held( &boom1PressedHeld );
     boom1.attach_deasserted_held( &boom1ReleasedHeld );
@@ -1303,11 +1331,12 @@
 filterTicker.attach(&filter, FILTER_RATE);
 angle_print.attach(&toprint,0.2);
     activate_antenna();
+    
     while(1) 
     {
     
         //JH send version information every 10 seconds to keep Bluetooth alive
-        if ((vTimer.read_ms()-lastsend_version)>15000) 
+        if ((vTimer.read_ms()-lastsend_version)>25000) 
         {
        
             pc.puts(version);
@@ -1356,7 +1385,7 @@
        }
        else ledRED=0;
        
-       if ( ((motTimer.read_ms()-lastsend_motorstate)>5000) || (motor_enable!=lastmotor_enable)) 
+       if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) 
         {
        
             bluetooth.puts(motor_enable_state);