Befehle, die nur zur Initialisierung dienen, sind:
setServoPower(1);
initSERVO(SERVO1 | SERVO2 | SERVO3 );

Die müssen VOR der while(true)-Schleife stehen.
Man braucht sie auch nicht 2x hintereinander aufzurufen.