clear value when xbee data stop

include/main.h

@@ -262,7 +262,9 @@ void setVelocity()
         unpacker.pop();
         unpacker.pop();
     }
     }
 
 
-    if (stopwatch.isRunning() && (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS))
+    if (stopwatch.isRunning())
+    {
+        if (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS)
         {
         {
             if (b_motor_active)
             if (b_motor_active)
             {
             {
@@ -270,9 +272,17 @@ void setVelocity()
             }
             }
             else
             else
             {
             {
+                vel[0] = vel[1] = vel[2] = 0.f;
                 Serial.println("Emergency Switch Pressed!! Motor Disabled...");
                 Serial.println("Emergency Switch Pressed!! Motor Disabled...");
             }
             }
         }
         }
+        else
+        {
+            vel[0] = vel[1] = vel[2] = 0.f;
+            stopwatch.stop();
+            Serial.println("XBee Communication Timeout...");
+        }
+    }
 }
 }
 
 
 void sendStatus()
 void sendStatus()