Die Servo Lib benutzt meines Wissens nach Software PWM, schaltet die Pins also "manuell" an und aus

Du könntest dich evtl. mal mit den Timern für Atmegas beschäftigen, die haben i.d.R. mind 2 Timer mit jeweils 2 PWM Ausgängen, die deine Servos, ohne auch nur ein einziges Stück Code (ausgenommen Initialisierung und bei Änderung der Position), ansteuern können.

Der Timer zählt dabei einfach nur hoch und wenn der Timer dabei den WErt des Compare REgister des Ausgangs erreicht schaltet er den zugehörigen Pin an oder aus, je nachdem wie der Timer und die PWM konfigurtiert ist.

Du musst dann praktisch nur das Compare Register mit 2-3 Prozessorzyklen ändern um den Servo passend zu verstellen (ausgenommen die Berechnung deiner Zielposition)

PS leider ist das Pin Mapping limitiert und du musst vermutlich einige Pins umplanen und die Progammierung wird dann auch schwer in Richtung "Bare Metal" kippen, also andere Librarys verwenden wird dann evtl. Probleamtischer ... oder du findest heraus mit welcher anderen SErvo Lib oder wie du mit der bestehenden Servo Lib die Hardware PWM nutzen kannt um das Problem zu minimieren.