Die Methode task_SERVO() wird exakt einmal ausgeführt, sie sollte aber vermutlich häufiger aufgerufen werden um die Position des Servos zu verändern.
Entweder arbeitest du mit einer Statemachine und setzt Ereignisse für jeden Fahrbefehl oder du rufst die Fahrbefehle mit blocking=false auf und packst hinter jeden Fahrbefehl eine
Code:
while(!isMovementComplete()){
   task_SERVO();
}
Schleife.

Grüße,
Jan

P.S.: mit [ code ] und [ /code ] wird der Quelltext auch schön dargestellt