Könntet ihr euch bitte mal meinen Servocode ansehen?

Code:
#include <nibobee/iodefs.h>
#include <nibobee/delay.h>
#include <nibobee/analog.h>
#include <stdlib.h>  

unsigned char i, servo_stellzeit;

void servo(unsigned char winkel) 
{ 
   unsigned int count=0; 
   do
   { 
      count++; 

      if(winkel)
	  { 
      PORTC |= (1 << PC3); 
      delay(winkel); 
      }
	  PORTC &= ~(1 << PC3); 

      delay(255-winkel); 
   }
   while (count<servo_stellzeit); 
} 

int main(void) 	
{

	analog_init();
	
	    while(1)
		{
		servo_stellzeit=150;
		delay(100);
		for (i=15; i<85; i+=2) servo(i);
		delay(100);
		for (i=75; i>17; i-=2) servo(i);
		delay(100);
		}
   return 0;
}
Der Servo zuckt bei diesem Programm in 1 Grad-Schritten, sollte aber zwischen zwei Positionen hin- und herdrehen...