     if (servo.read() > argument) {
       waitTimeInMs = servo.read() - argument;
     } else {
-      waitTimeInMs = servo.read() + argument;
+      waitTimeInMs = argument - servo.read();
     }
-
+    
+    waitTimeInMs = (waitTimeInMs + 100) * 2;
+    
     servo.write(argument);
-    
-    Serial.print(waitTimeInMs);
-    
-    delay(waitTimeInMs * 3);
-    
+
+    // give the servo some time to do its job
+    delay(waitTimeInMs);
+
+    // we detach the servo variable from its pin to save energy
     servo.detach();
     
     digitalWrite(ledPin, LOW);

  
  String command = getCommandFromInput(input);
  int argument = getArgumentFromInput(input);
  
  if(command == "battery") {
    result = checkBatteryStatus(argument);
  } else if(command == "distance") {

    result = halt(argument);
  } else if(command == "debug") {
    result = debug(argument);
  // here we can add additional functions:
  // } else if(command == "doSomething") {
  //   result = doSomething(argument);
  } else {
    result = "unknown command: " + command;
  }
    
  return result;
