Servo über IOs steuern

Hi,:-)
ich hab ein kleines programm zum steuern eines servos mit den IOs auf dem m32 geschrieben:

void servo(uint8_t state1,uint8_t state2)
{
DDRC |= IO_PC4;
if (getStopwatch3() >= 20)
{
for ( state1 = state1; state1 > 0; state1-- )
{
PORTC |= IO_PC4;
sleep(1);
}
for ( state2 = state2; state2 > 0; state2-- )
{
PORTC &= ~IO_PC4;
sleep(1);
}
setStopwatch3(0);
}
}

void servo_stellzeit(uint8_t state1,uint8_t state2 ,uint16_t stellzeit)
{
while(stellzeit > 0)
{
servo(state1,state2);
mSleep(20);
stellzeit--;
}
}

damit kann man dem servo sagen bis wohin er sich drehen soll.
Jetzt hab ich eine Frage:
Wie kann man bestimmen wie schnell er sich drehen soll ?

DIV

Gruß blenderkid