   if (adk.isReady()) {
       adk.read(&bytesRead, BUFFSIZE, buffer);
     if (bytesRead > 0) {
-      jeepCommandInterpreter(buffer[0], buffer[1]);
+      jeepCommandInterpreter(buffer[0] - 48, buffer[1] - 48);
     }
   }
 }
 
-void jeepCommandInterpreter(uint8_t command, uint8_t speed) {
-  switch(command) {
+void jeepCommandInterpreter(uint8_t commandMovement, uint8_t commandSpeed) {
+  int vPower = speedToPower(commandSpeed);
+  Serial.print("Required power: ");Serial.println(vPower);
+
+  switch(commandMovement) {
     case 0:
-      goForward(speed);
+      Serial.println("Received command: 0 -> move forward");
+      goForward(vPower);
       break;
     case 1:
-      goBackward(speed);
+      Serial.println("Received command: 1 -> move backward");
+      goBackward(vPower);
       break;
     case 2:
-      goLeft(speed);
+      Serial.println("Received command: 2 -> turn left");
+      turnLeft(vPower);
       break;
     case 3:
-      goRight(speed);
-      break;
+      Serial.println("Received command: 3 -> turn right");
+      turnRight(vPower);
+      break;
+    case 4:
+      Serial.println("Received command: 4 -> turn back");
+      turnBack(vPower);
+      break;
+    case 5:
+      Serial.println("Received command: 5 -> testing all movements");
+      testAllMovements(vPower);
     default:
       Serial.println("Command not available");
       break;

}

// Main movement command
void goForward(uint8_t speed) {
}

void goBackward(uint8_t speed) {
  turnBack();
  goForward(speed);
}

void goLeft(uint8_t speed) {
  turnLeft();
  goForward(speed);
}

void goRight(uint8_t speed) {
  turnRight();
  goForward(speed);
}

// Helper movement
int speedToPower(uint8_t speed) {
  int v;
  switch(speed) {

  
  // Return a safe motor power
  return motorProtection(v);
}

// Emergency checks
