Hi topgunfb,

was soll dein Programm denn machen?
Du gibst allen Servos Werte (0), dann nach 50ms ARM_HOCH und ARM_SEIT zwei neue Werte (60, 80), die dann so bleiben.

1. Zum Testen würde ich für die Stopwatch2 z.B. Werte von 5000 (anstelle von 48 ) nehmen, damit jede Stellung 5s gehalten wird.

2. In dem if (getStopwatch2() > 5000) Teil würde ich als Test einen Zähler setzen und bei jedem Durchlauf (also alle 5s) per Zähler den Servos neue Werte zuweisen:
Wenn Zähler = 1 -> pos = 30
Wenn Zähler = 2 -> pos = 50 usw. (z.B. als switch-Konstrukt)

3. setStopwatch2(0) gehört ans Ende des if (getStopwatch2() > 5000) Teils, nicht in die Hauptschleife mit task_SERVO().

4. Irgendwie ist eine "}" zuviel. Da müßte der Compiler meckern.

Gruß Dirk