versuchs mal mit diesem code hier.
Code:
#include "RP6ControlLib.h" 


void servo_io5 (uint8_t imp)
	{
    DDRC |= IO_PC5;
    PORTC |= IO_PC5;   
    sleep(imp);         
    PORTC &= ~IO_PC5;
    sleep(200-imp);       
	} 




int main(void)
{
	initRP6Control();
	initLCD(); 
	
	
	uint8_t i=0, x;
	
	i=0;
	x=10;
	
	while(true)
	{
		while(i<10) //200ms
		{
		servo_io5(x);
		i=i+1;
		}
	x=x+1;
		if (x>=20)
		{
		x=10;
		}
		i=0;
	}


	
	
	
	while(true) 
	{
		showScreenLCD("Endlosschleife", "");
	}
	return 0;
}
Ist die Grundkonstruktion (funktioniert auch) für die Steuerung eines Servos.

Versuch einfach das ein Wenig umzuschreiben.

PS:
Schau in den Thread.