Mal so als Anfang: "Print" verlangsamt deinen Code ziemlich stark (fast so wie waitms 1). Wenn du den Print Befehl rausnimmst und statt dessen eine Servoposition stellst, dann könnte es sein dass dein Code einfach zu schnell läuft. Versuchs doch mal ohne den Servobefehl (mit einer eigenen Servo Routine) oder verlangsame deinen Code per Waitms oder so.