Hallo,

bisschen ungewöhnlicher Ansatz
Von deiner Kontroll-LED wirst du nicht viel sehen - höchstens mit m Oszi und dann kannst du auch den Servopin beobachten.
Wenn ich das richtig aus deinem mainloop interpretiere gibst du nicht wirklich n richtiges Servosignal aus.
Dabei sollte eine Pause zwischen den Impulsen sein (Periodendauer 20ms, Servoimpuls 1-2ms).

Ich habe das mit dem Timer 1 gemacht (ATmega. Damit kannst du verschiedene Interrupts erzeugen (1x Overflow und 2x Compare). Habe den Timer so eingestellt, dass er alle 20ms einen Overflow hat und dabei den Ausgang/die Ausgänge von den Servos auf high setzen. Dann muss nur noch der/die Compare Register richtig geladen werden, damit der Impuls in dem Bereich liegt, und mit ihnen der Ausgang wieder auf low gesetzt wird.

Damit kannst du mit dem Timer 2 Servos relativ "Arbeitssparend" außerhalb dem Hauptprogramm ansteuern.

MFG Mixxer