AT90 Servo-Pulsgenerator pulst ungleichmäßig

#include <avr/io.h>

#include <avr/interrupt.h>

#define F_CPU 16000000ULL

#include <util/delay.h>

void Reg_Init(void)

  {

  TCCR0A=0b00000101; // kein PWM,kein Compare Match , prescaler 1024 -> 2ms = 30 Pulse

  TIMSK0=0b00000000; // erstmal kein Compare Match interrupt

  OCR0A=15;

  }

void sleep(uint32_t i) 

{

  while(i--)

    _delay_ms(1);

}

//Variablen

void PORT_Init()

{

  PORTA = 0b00000000;

  DDRA = 0b00000000;

  PORTB = 0b00000000;

  DDRB = 0b00000000;     

  PORTC = 0b00000000;

  DDRC = 0b00000000;

  PORTD = 0b00000000;

  DDRD = 0b00000100;     

  PORTE = 0b00010000;      

  DDRE = 0b00010011;    //Led4+servo78 set as output 

  PORTF = 0b00000000;

  DDRF = 0b00000000;

  }

void WDT_off(void)

  {

  cli();

  /* Write logical one to WDCE and WDE */

  WDTCR = (1<<WDCE) | (1<<WDE);

  /* Turn off WDT */

  WDTCR = 0x00;

  }

int main()

  {

  WDT_off();

  PORT_Init();

    Reg_Init();

  sei();  // Alles Initialisiert, Interupts Freischalten

  int i;

  int j;

  while (1)

    {

      sleep(25);

      OCR0A=16;

      PORTE  &=0b11101111; //LED AN

      PORTE |= 0b00000011; //SERVO PULS AN

      TCNT0=0;

      TIFR0 = 0b00000010; // Pending Interrputs löschen

      TIMSK0=0b00000010; // Compare Match 0 interrupt an

    }

  }

ISR(TIMER0_COMP_vect)  // 2ms-timer interrupt

  { 

  PORTE |= 0b00010000; //LED AUS

  PORTE  &=0b11111100; //SERVO PULS AUS

  TIMSK0 &=0b11111101; // Compare Match 0 interrupt aus

  }