           break;
         }
         lastCommand=command;
-        } else {
+      } 
+      else {
+        if (MOV_DEBUG_MODE){
           Serial.print("ERROR: Command "); Serial.print(command); Serial.print(", param1 "); Serial.print(param1); Serial.print(", param2 "); Serial.print(param2); Serial.println(", is not valid!");
         }
-        } else {
-          Serial.print("*");
-        }
-        sendToADK( random(0, 2), random(0, 4) + random(1, 3) * 100, random(150, 152));
       }
-      delay(DELAY);
-    }
-
-    char* readFromADK() {
-      adk.read(&bytesRead, BUFFSIZE, inBuffer);
-      memset(inStringBuffer, 0, BUFFSIZE);
-      if (bytesRead > 0) {
-        memcpy(inStringBuffer, inBuffer, bytesRead);
-        if (COM_DEBUG_MODE) {
-          Serial.print("\nReceiving | ");
-          Serial.print(inStringBuffer);
-          Serial.print(" | ");
-          Serial.print(strlen(inStringBuffer));
-          Serial.println(" bytes incoming");
-        }
-      }
-      delay(DELAY / 10);
+    } 
+    else {
+      Serial.print("*");
+    }
+    sendToADK( random(0, 2), random(0, 4) + random(1, 3) * 100, random(150, 152));
+  }
+  delay(DELAY);
+}
+
+char* readFromADK() {
+  adk.read(&bytesRead, BUFFSIZE, inBuffer);
+  memset(inStringBuffer, 0, BUFFSIZE);
+  if (bytesRead > 0) {
+    memcpy(inStringBuffer, inBuffer, bytesRead);
+    if (COM_DEBUG_MODE) {
+      Serial.print("\nReceiving | ");
+      Serial.print(inStringBuffer);
+      Serial.print(" | ");
+      Serial.print(strlen(inStringBuffer));
+      Serial.println(" bytes incoming");
+    }
+  }
+  delay(DELAY / 10);
   return inStringBuffer; // If nothing has been read then return the previously initialized empty array
 }
 

    char*a;
    if (strlen(a = readFromADK()) > 0) {
      if (decodeCommand(a, &command, &param1, &param2)) {
        Serial.print("COMMAND: Command "); Serial.print(command); Serial.print(", param1 "); Serial.print(param1); Serial.print(", param2 "); Serial.print(param2); Serial.println(", received and is beign processed!");
        switch (command) {
          case CMD_LEFT:
          turnLeft(param1);
          break;
          case CMD_RIGHT:
          turnRight(param1);
          break;
          case CMD_STOP:
          stop(SOFT);
          break;
          default:
          stop(SOFT);

    analogWrite(PWM_LEFT, LOW);
    analogWrite(PWM_RIGHT, LOW);
    } else{
      currentRightVelocity=currentLeftVelocity=LOW;
      setSpeedAndGo(LOW,LOW);
    }
    delay(DELAY / 10);
