Sortieralgorithmen dürften sich gewaltig lohnen - wenn immer nur ein Servo seinen Wert ändert (und diesen womöglich auch nur wenig)

Wenn du vier Servos mit unterschiedlichen Weglängen hast, sind spätestens nach dem ersten Sortierdurchgang alle sortiert.

Dann nimmst du das "oberste" und bewegst es ein bisschen

Dann schaust du nach, ob es noch das "oberste" ist (vergleiche es mit dem nächstunteren)- wenn nicht sortierst du es entsprechend nach unten.

Das nächste mit dem "oberste" liegt dann schon bereit.

Du musst dann nur eine Array-Organissation finden, mit der du nicht immer alles neu herumschieben musst. (verkettete List z.B. oder direktadressierte Elemente)

ärgerlich ist dann nur, wenn die Sollwerte geändert werden und alle Servos neu sortiert werden müssen. Aber so oft wird das vielleicht nicht vorkommen?