@Philip:
... der Servo bewegt sich nicht
Das liegt daran, dass die move-Funktion die while(true)-Schleife so lange blockiert, bis die Bewegung abgeschlossen ist.
Die task_SERVO() braucht aber eine while(true)-Schleife, die permanent durchläuft und nur wenige ms dauert.

Eine Lösung wäre, die move-Funktion nicht BLOCKING zu machen (also: move(60, FWD, DIST_CM(30), NON_BLOCKING) ) und direkt hinter der move-Funktion einzufügen:
Code:
 while(!isMovementComplete()) 
 {
  task_checkINT0();
  task_I2CTWI();
  task_SERVO();
 }
Damit wird in der Schleife auch gewartet, bis die Bewegung beendet ist, aber trotzdem werden die 3 wichtigen Tasks aufgerufen.
Nicht getestet! Schlag mich nicht, wenn's nicht geht!