   {
     motor1_target = bctoi(2, 3);
     motor2_target = bctoi(5, 3);
-    if (motor1_target > MOTOR_VELOCITY_0 + MOTOR_VELOCITY_MAX) motor1_target = MOTOR_VELOCITY_0 + MOTOR_VELOCITY_MAX;
-    if (motor2_target > MOTOR_VELOCITY_0 + MOTOR_VELOCITY_MAX) motor2_target = MOTOR_VELOCITY_0 + MOTOR_VELOCITY_MAX;
-  }
-  else
-  if (cmd == 'Z' || cmd == 'z') // stop immediately
-  {
-    motor1_target = MOTOR_VELOCITY_0;
-    motor1 = MOTOR_VELOCITY_0;
-    motor2_target = MOTOR_VELOCITY_0;
-    motor2 = MOTOR_VELOCITY_0;
-    roboclaw.ForwardBackwardM1(RC_ADDR, motor1);
-    roboclaw.ForwardBackwardM2(RC_ADDR, motor2);
-  }
+    if (motor1_target > MOTOR_TARGET_MAX) motor1_target = MOTOR_TARGET_MAX;
+    if (motor2_target > MOTOR_TARGET_MAX) motor2_target = MOTOR_TARGET_MAX;
+  }
+  else
+  if (cmd == 'Z' || cmd == 'z') driveStop(); // stop immediately
   else
   if (cmd == 'F' || cmd == 'f') { motor1_target = MOTOR_VELOCITY_0 + motor_velocity; motor2_target = motor1_target; }
   else

    buf[bytes] = Serial.read();
    if (buf[bytes] == 10 || buf[bytes] == 13 || bytes >= MAX_COMMAND)
    {
      if (bytes > 0) Execute();
      //if (bytes > 0) if (crc8_ok()) Execute();
      return;
    }
    bytes++;

{
  driveStop();
  IsConnected = false;
}

void OnValidCommand()
{
  IsConnected = true;
  lastCommandTime = millis();
}

//=======================================
