... Sleep(2). Lässt deinen derzeitigen Thread schlafen ... Das sollte nicht nötig sein ...
Ich kann nicht speziell auf Deinen Code eingehen. A) ich habe noch nie nen Hexa programmiert und B) Mein C ist eher bescheiden.

Aber von mehreren Servos "simultan" ansprechen verstehe ich schon ein bisschen. Nur sieht meine Lösung völlig anders aus, ich erledige das sozusagen nebenher, in einer ISR - mit dem Timer 1 (mega1284, 20 MHz). Hier mal skizziert, wie das für 8 Servos aussähe - bei mir laufen die Zeiten etwas anders, weil ich zehn Servos bediene.

Der Timer 1 läuft so, das sein Kanal A (OCR1A) pro Sekunde runde 8 x 50 = 400 Interrupts auslöst (COMPARE A). In der angstossenen ISR wird der Servopointer "berechnet" und damit dann der der aktuell angewählte Servo gestartet sowie der Kanal B gestartet mit der Zeit, die der angewählte Servo zu laufen hat (OCR1B). OCR1B MUSS immer einen Klacks kleiner sein als OCR1A - daher kommt die ISR COMP B noch vor der nächsten ISR COMP A. Dieser Wert von OCR1B wird im Wertefeld Srv_tm [i] abgelegt {i ε 1 .. 8:). In der folgenden ISR COMP B wird der aktuelle Servo gestoppt (bei mir sicherheitshalber - falls Überschneidungsprobleme auftauchen auch der Servo "davor"). Danach kommt die nächste ISR COMPA - (und weiter gehts mit "oben" >>..wird der Servopointer "berechnet"..<<). Nachdem die ISR COMPA A acht mal durchgelaufen ist, sind runde 20 ms vergangen - die gewünschte Periodenzeit für einen Servo; es kommt also jeder Servo alle 20 ms EINMAL dran.

Fazit: die Servos werden nur durch Vorgabe der jeweiligen Werte OCR1A und OCR1B bedient, der Rest läuft selbst - ohne irgendein sleep. So geht das bis alle acht Servos bedient sind.

Die Servos erhalten die Werte von "irgendwoher" - hier könnte Deine Matritze abgefragt werden - Du solltest dann aber vermutlich bei jedem Rundumschlag der ISR COMP A ==> EINMAL ein Flag setzen, damit Du die Werte wenigstens einmal pro Durchgang benutzt.

Laufen tut das dann z.B. um die zehn Servos im Kopf meines Roboters zu bedienen. Dort, in diesem Posting, siehst Du auch meine Liste verschiedener Flags, mit denen ich zusätzlich noch verschiedene Zustände der Servos schalte.

Wie betont/geschrieben - ich habe noch nie nen Hexa programmiert, dort gibt es sicher andere, klevere Lösungen.