Библиотека RadioHead изменяет состояние при получении нового сообщения

Я хотел бы управлять игрушечной машинкой с сервера. Я использую библиотеку RadioHead для общения, и она работает нормально.

Приложение, запущенное на стороне сервера, выглядит следующим образом.

#include <RHReliableDatagram.h>
#include <RH_ASK.h>
#include <SPI.h>

#define SERVER_ADDRESS 1
#define ROBOT_ADDRESS 2
#define GO_RIGHT "Go right"
#define GO_LEFT "Go left"
#define GO_FORWARD "Go forward"
#define GO_BACK "Go back"
#define STOP "Stop"
#define SERVER_DELAY 500

unsigned int transmitterPin = 10;
unsigned int receiverPin = 12;

RH_ASK transmitterServer(2000, receiverPin, transmitterPin, 0);

RHReliableDatagram radioCommunication(transmitterServer, SERVER_ADDRESS);

void setup()
{
    Serial.begin(9600);
    if (!radioCommunication.init())
        Serial.println("Radio communication failed");
}

uint8_t buffer[RH_ASK_MAX_MESSAGE_LEN];

void sendData(uint8_t * data, uint8_t length)
{
    Serial.println("I'm sending commands to the robot");

    // Отправить команду роботу
    if (radioCommunication.sendtoWait(data, length, ROBOT_ADDRESS))
    {
        // Теперь ждите ответа от робота
        uint8_t from;
        if (radioCommunication.recvfromAckTimeout(buffer, &length, 2000, &from))
        {
            Serial.print("I've got - ");
            Serial.print((char*) buffer);
            Serial.print(" - message from the robot whose addres is : ");
            Serial.print("0x");
            Serial.println(from, HEX);
        }
        else
        {
            Serial.println("No response. Does the robot works?");
        }
    }
    else
        Serial.println("Attempt to resend data failed.");
    delay(SERVER_DELAY);
}

void loop()
{
    sendData(GO_RIGHT, sizeof(GO_RIGHT));
    sendData(GO_LEFT, sizeof(GO_LEFT));
    sendData(GO_FORWARD, sizeof(GO_FORWARD));
    sendData(STOP, sizeof(STOP));
    sendData(GO_BACK, sizeof(GO_BACK));
    sendData(STOP, sizeof(STOP));
}

Приложение, запущенное на роботе, выглядит следующим образом.

#include <RHReliableDatagram.h>
#include <RH_ASK.h>
#include <SPI.h>

#define SERVER_ADDRESS 1
#define ROBOT_ADDRESS 2
#define GO_RIGHT "Go right"
#define GO_LEFT "Go left"
#define GO_FORWARD "Go forward"
#define GO_BACK "Go back"
#define STOP "Stop"
#define RIGHT_LIGHT 4
#define LEFT_LIGHT 7
#define FORWARD_LIGHT 8

unsigned int receiverPin = 2;
unsigned int transmitterPin = 12;

RH_ASK robotTransmitter(2000, receiverPin, transmitterPin, 0);
RHReliableDatagram radioCommunication(robotTransmitter, ROBOT_ADDRESS);

void setup()
{
    Serial.begin(9600);
    pinMode(RIGHT_LIGHT, OUTPUT);
    pinMode(LEFT_LIGHT, OUTPUT);
    pinMode(FORWARD_LIGHT, OUTPUT);

    if (!radioCommunication.init())
        Serial.println("Radio communication failed");
}

uint8_t buffer[RH_ASK_MAX_MESSAGE_LEN];
uint8_t command[RH_ASK_MAX_MESSAGE_LEN];

void moveRight()
{
    Serial.println("------------");
    Serial.println("I'm going right");
    Serial.println("------------");

    /*
       // Трудоемкая задача

    for (int i = 0; i < 3; ++i)
    {
        digitalWrite(RIGHT_LIGHT, HIGH);
        delay(1000);
        digitalWrite(RIGHT_LIGHT, LOW);
        delay(1000);
    }

    */
}

void moveLeft()
{
    Serial.println("------------");
    Serial.println("I'm going left");
    Serial.println("------------");

    /*
       // Трудоемкая задача

    for (int i = 0; i < 3; ++i)
    {
        digitalWrite(LEFT_LIGHT, HIGH);
        delay(1000);
        digitalWrite(LEFT_LIGHT, LOW);
        delay(1000);
    }

    */
}

void runCommand(char* command)
{
    if (strcmp(command, GO_RIGHT) == 0)
    {
        moveRight();
    }
    else if (strcmp(command, GO_LEFT) == 0)
    {
        moveLeft();
    }
}

void loop()
{
    uint8_t response[] = "OK";
    if (radioCommunication.available())
    {
        uint8_t length = sizeof(buffer);
        uint8_t from;
        if (radioCommunication.recvfromAck(buffer, &length, &from))
        {
            Serial.print("I got - ");
            Serial.print((char*) buffer);
            Serial.print(" - command from the server whose addres is ");
            Serial.print("0x");
            Serial.println(from, HEX);
        }
        // Отправьте сообщение на сервер (с повторными попытками) и дождитесь подтверждения.
        if (!radioCommunication.sendtoWait(response, sizeof(response), from))
            Serial.println("Repeated attemps to send data to server failed");
        strncpy(command, buffer, sizeof(command));
        runCommand(command);
    }
}

В нынешнем виде последовательный вывод robot выглядит следующим образом:

Давайте увеличим переменную SERVER_DELAY до 2000. Таким образом, сервер будет отправлять команду каждые две секунды.

Кроме того, давайте выполним команду, которая займет некоторое время у робота. Для этого мы могли бы раскомментировать строки задач, отнимающие много времени.

Таким образом, когда робот получит команду "Идти правильно", он попытается включить и выключить свет три раза.

После внесения этих изменений давайте посмотрим на серийный выпуск robot.

Что происходит, так это то, что после получения команды Go left он пытается мигнуть LEFT_LIGHT три раза. Но в то же время он пропускает команду "Вперед". И снова после получения команды Go right он пытается мигнуть RIGHT_LIGHT три раза, но пропускает команду Go left.

Что я хотел бы сделать, так это когда робот получает новую команду, он должен прекратить все, что он делал, и выполнить новую команду.

Как я должен это сделать? Есть какие-нибудь идеи. Спасибо.