Wo hakt es denn genau?
Die einzelnen Zeilen sind doch relativ gut dokumentiert.
Zuerst wird der Prescaler für den Timer berechnet und mit den #defines zugewiesen.
In der TIMER0_COMP_vect werden die einzelnen Servoimpulse nacheinander ausgegeben.
In der InitServo wird der Timer 0 und die benötigten Servoausgänge initialisiert.

In der Hauptschleife werden die Variablen für die Servoimpulslänge gesetzt.

Was mir bei den ganzen Sache fehlt sind die Pausen.
Normale Servos wollen zwischen zwei Impulsen eine Pause von 20ms haben.
Bei speziellen "schnellen" Servos dürfen es auch 10ms sein.
Das scheint mir auf den ersten Blick in diese Routine nicht implementiert zu sein.