das sollte dir helfen: http://users.4finger.net/~smay/avr/servomaster/
Ich kann auch daheim nochmal schaun ob ich meine alten Sourcen noch habe...hab vorm halben Jahr genau sowas gemacht....

Gruss
Michael