Mach die Servo-Ansteuerung lieber über hardware-PWM an den Pins OC1a und OC2.
Lied dir dazu im Datenblatt des Controllers den Artikel "Timer/counter" (oder so ähnlich) durch, dann brauchst du nicht mal Interrupts.
Ich würde Fast-PWM empfehlen (ich finde, das kann man am schönsten einstellen.

Grüße, Yaro