Auch ohne gleich ein Linux aufspielen zu müssen, kann man sich mit einem relativ einfachen Scheduler ein durchaus brauchbares Multitasking für einen ATMega bauen.
Man findet da ne ganze Menge Beispiele im Netz...
Wenn's nicht unbedingt ein eigener Scheduler sein soll, dann könnte sich ein Blick auf FreeRTOS o.Ä. lohnen.
Dafür gibt es auch schon Ports für den ATMega, ggfs. muss man dort noch manuell ein Paar Anpassungen machen.
Ein weiterer Vorteil von FreeRTOS ist die Portabilität auf andere µC-Familien, die High-Level-Programmierung
funktioniert auf einem ATMega (fast) genauso wie etwa auf einem ARM Cortex Mx µC.

Das wäre allerdings die "Mit Atombomben auf Ameisen geschossen"-Lösung, die auch nicht gerade die Performanteste ist.

Statt die Servos nur zu gewissen Zeiten in irgendeinem Unterprogramm anzusteuern bietet es sich doch an, die eigentliche Servosteuerung (->Pulsgenerierung) interruptgesteuert mit einem
der Timer des ATMega zu erledigen. In Deinen Unterprogrammen berechnest Du dann die neuen Sollwerte, welche dann in der ISR an die Servos weitergegeben werden.
Vermutlich macht Deine Library genau dies - poste doch mal die Inhalte der Servo.h und Servo.c. Ein Auszug aus Deinem Hauptptogramm (wo setup() und loop() aufgerufen werden könnte auch noch hilfreich sein.

Grüße,

Max