           Serial.print("ERROR: Command "); Serial.print(command); Serial.print(", param1 "); Serial.print(param1); Serial.print(", param2 "); Serial.print(param2); Serial.println(", is not valid!");
         }
       }
+      stateYellow=false;
+      digitalWrite(LED_YELLOW, stateYellow);
       if (millis()-blinkGreenTimer>300){
         stateGreen=!stateGreen;
         blinkGreenTimer=millis();

  delay(1000);
  Serial.println("All power to the engines!");
  stop(SOFT);
  blinkGreenTimer=blinkRedTimer= millis();
  stateGreen=stateRed=true;
  emergency_mode=warning=false;
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_YELLOW, LOW);


#include <stdio.h>
#include <adk.h>
#define LED_GREEN 22
#define LED_YELLOW 24
#define LED_RED 26
#define BUFFSIZE   255
#define MAX_POWER  400
#define DELAY 50 // Smaller delay breaks everything about communication
