Ach ja ich benutze timer 2 fur die schleife bei 4mhz
1000000:16:125:10 is bei mir 50 Hz und das sind 20 ms
als main wird ein counter hochgezähl der aller 5 µs die Servos abfagt und bei ereichen des stellwertes diese abschaltet.
der init macht die wieder an in 5µs abständen.und nimm nich das return beim init
wies in c geht keine ahnung ich progge asm