Mini-Logik Frage - Delayfunktion für viele Sensorwerte

Hallo Zusammen,

ich habe eine Regelung die sich aufschaukelt (Gyro und Servo),
da das Servo natürlich ein paar Millisekunden (~40-70ms) braucht
um die Befehle umzusetzen.

Die Lösung wäre, die Sensorwerte um 40-70ms zeitverzögert auszugeben.
Jetzt hab ich irgendwie einen kleine Knoten im Hirn :)
Wie macht man das am Besten (frei skalierbar).

Speicher ich alle erkannten werden in einem Array ab, und je
nach Skalierung greife ich die Werte unterschiedlich spät ab.

Ok.

Nur dann muß ich ja konstant das Array aktualisieren,
das heißt, alle einträge müßten um eins nach hinten
gesetzt werden.

70x die Aktion (alle Arrayeinträge verschieben) und am
Anfang ein neuen Werte einfügen und den letzen löschen
ist irgendwie nicht besonders intelligent... :)

Hat hier jemand vielleicht eine Idee wie man das besser
macht?

Danke & Viele Grüße
Bernd

(ich vermute mal, das geht um den Faktor 70 einfacher,
nur kommt ich da nicht drauf ... :))