clear value when xbee data stop

include/main.h

@@ -262,15 +262,25 @@ void setVelocity()
         unpacker.pop();
         unpacker.pop();
     }
     }
 
 
-    if (stopwatch.isRunning() && (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS))
+    if (stopwatch.isRunning())
     {
     {
-        if (b_motor_active)
+        if (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS)
         {
         {
-            if (fps.isNextFrame()) whill.move(vel[0], vel[1], vel[2]);
+            if (b_motor_active)
+            {
+                if (fps.isNextFrame()) whill.move(vel[0], vel[1], vel[2]);
+            }
+            else
+            {
+                vel[0] = vel[1] = vel[2] = 0.f;
+                Serial.println("Emergency Switch Pressed!! Motor Disabled...");
+            }
         }
         }
         else
         else
         {
         {
-            Serial.println("Emergency Switch Pressed!! Motor Disabled...");
+            vel[0] = vel[1] = vel[2] = 0.f;
+            stopwatch.stop();
+            Serial.println("XBee Communication Timeout...");
         }
         }
     }
     }
 }
 }