Hi,
was hälst du davon, deiner task_SERVO(void) Methode die Position des Servos als Parameter zu übergeben? Dadurch siehst du im Programm schneller, an welcher Stelle der Stift angehoben, bzw. abgesenkt wird und musst nicht die Aufrufe von task_SERVO() zählen.

Grüße,
Jan