Hi,

okay, jetzt weiß ich, was Du mit den 16 ms meintest. Dein Interrupt wird alle 16 ms ausgelöst, das hat aber nix mit der Dauer der ISR zu tun! Wenn Du mit 16 MHz taktest braucht Dein count++ definitiv keine 16 ms (weiß nicht, wie viele Maschinenzyklen ein ADD braucht, aber wenn wir mal von 4 ausgehen wären das 250 ns für den ADD, dann kommt noch a bisserl was für das PUSH und POP dazu, also wenn ganz schlimm kommt sagen wir mal 5 µs)!

Deine Motorsteuerung alleine klappt auch, oder hast Du das noch nicht versucht?
Wie lange braucht denn Dein AD-Wandler, bis seine Wandlung durchgeführt ist?

Noch was: Wenn Du in mehr als einer Funktion auf Variablen zugreifst solltest Du diese als volatile anlegen. Das bewirkt, dass der Compiler nicht mit irgendwelchen lokal gespeicherten Werte arbeiten darf, sondern jedesmal auf die Speicheradresse, an der die Variable liegt, zugreift. Das wäre bei Dir z.B. bei count der Fall. (Konkret heißt das also dann "volatile uint8_t count = 0")

Und wie gesagt, lass mal das cli() in der while(1) weg, das gehört da nicht hin (stört ja nicht weiter, wenn das Warten auf die Wandlung durch den Timer Interrupt unterbrochen werden sollte).
Dann würde ich auch in der Funktion IR_Servoposition auf count >= 2 abfragen und nicht auf count == 2.

Viele Grüße