Ich hab's nicht getestet:

Code:
// "Weiche" Servoansteuerung mit sleep()                             mic 30.8.10

#include "RP6ControlLib.h"

char servo1(int pos)
{
   static char pos_temp=0; // static = Werte zwischen den Aufrufen speichern
   static char stellzeit=0;// dito

   if (!pos_temp)    		// pos_temp beim ersten Aufruf initialisieren
   {
      pos_temp=pos;  		// Schnell zur gewünschten Position fahren
		stellzeit=50;  		// innerhalb einer Sekunde
   }

	PORTC |= IO_PC4;			// Impuls für aktuelle Position senden
   sleep(pos_temp);
   PORTC &= ~IO_PC4;    	// Impulspause
   sleep(200-pos_temp);     // Edit: mSleep() wird zu sleep()

   if(stellzeit) // solange Stellzeit aktiv werden gleichlange Impulse gesendet
   {
      stellzeit--;
	}
	else                 	// Position weiterschalten
	{
	   stellzeit=5;     		// Stellzeit für den nächsten Teilschritt 0,1 Sek.
	   if(pos_temp<pos) pos_temp++; // Position noch zu klein
	   	else if(pos_temp>pos) pos_temp--; // Position noch zu groß
	   	   else return(0); 							// 0 bedeutet: "Ziel erreicht"
	}
	return(1); // 1 bedeutet: "Ziel noch nicht erreicht"
}

int main(void)
{
   initRP6Control();
   DDRC  |= IO_PC4;
   PORTC &= ~IO_PC4;
   while(true)
   {
		while(servo1(10)); // Solange auf Position 10 fahren bis 0 zurückkommt
      while(servo1(20)); // dito mit Position 20
   }
   return(0);
}