Проблема с роботом (ШИМ)

Chip type : ATmega16
Program type : Application
AVR Core Clock frequency: 8,000000 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 256
*****************************************************/

#include <mega16.h>

unsigned int adc_data1;
unsigned int adc_data2;
unsigned int adc_data3;
unsigned int i; //переменная для направления движения

// Timer 1 output compare A interrupt service routine
interrupt [TIM1_COMPA] void timer1_compa_isr(void)
{
if (i==0){ //прямо
if (PORTB==0b00001001) PORTB=0x00;
else PORTB=0b00001001;
}
else
if (i==1){ //налево
if (PORTB==0b00000101) PORTB=0x00;
else PORTB=0b00000101;
}
else
if (i==2){ //напрво
if (PORTB==0b00001010) PORTB=0x00;
else PORTB=0b00001010;
}
else
if (i==3){ //назад
if (PORTB==0b00000110) PORTB=0x00;
else PORTB=0b00000110;
}
}

//Процедура ШИМ
void Start_PWM(unsigned int ocr_data)
{
TCNT1=0; //сброс счётного регистра
OCR1A=ocr_data; //Настройка регистра сравнения
TCCR1B=0x05; //установка делителя частоты (запуск таймера)
while(TIFR != 0x80); //ожидания переполнения (TOV1==1)
TCCR1B=0x00; //выключение таймера
}

void Backward(unsigned int ocr_data)
{
i=3; //движение назад
Start_PWM(ocr_data); //Запуск ШИМ
}

void ReadADC_1(unsigned int adc_setup)
{
ADCSRA=0x87;
ADMUX=adc_setup;
ADCSRA.6=1;
while(ADCSRA.4==0);
adc_data1=ADCW;
}

void ReadADC_2(unsigned int adc_setup)
{
ADCSRA=0x87;
ADMUX=adc_setup;
ADCSRA.6=1;
while(ADCSRA.4==0);
adc_data2=ADCW;
}

void ReadADC_3(unsigned int adc_setup)
{
ADCSRA=0x87;
ADMUX=adc_setup;
ADCSRA.6=1;
while(ADCSRA.4==0);
adc_data3=ADCW;
}

void main(void)
{
// Declare your local variables here

PORTA=0x00;
DDRA=0x00;

PORTB=0x00;
DDRB=0xFF; //0b00001111

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 7,813 kHz
// Mode: ШИМ корректный по фазе
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: On
// Compare B Match Interrupt: Off
TCCR1A=0x03;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x10;

ACSR=0x80;
SFIOR=0x00;

// Global enable interrupts
#asm("sei")

while (1)
{
ReadADC_3(0b01000010);

if (adc_data3 > 300) Backward(43200);
else PORTB=0b00001001;
};
}

while(TIFR != 0x80);

delay_ms(1000);