@I♥ROBOTIC:

Sieht eigentlich gut aus.

Nur 2 Hinweise:

1. In der Servo-Lib dient LEFT_TOUCH nur zum Einstellen des linken Anschlags. Sonst braucht man das nicht mehr.
Der linke Anschlag ist einfach 0.
Also:
Anstelle von: servo1_position = LEFT_TOUCH;
... einfach: servo1_position = 0;

2. Die Stopwatch-Tests müßten so funktionieren, aber ich würde es "eindeutiger" mit Klammern machen:
Anstelle von: if (getStopwatch1() > 1000 && getStopwatch1() < 2000)
... lieber: if ((getStopwatch1() > 1000) && (getStopwatch1() < 2000))

Gruß Dirk