Hmm, soweit ich weiß gibt es dazu schon fertige Funktionen in der C-Control Lib. Such mal in der Hilfe der C-Control-IDE nach Servo_Init und Servo_Set

...

Gruß Frank