Драйвер двигателя

#define F_CPU 16000000UL
#include <avr/io.h>
#include <util/delay.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#define FOSC 16000000UL // Clock Speed
#define BAUD 9600
#define MYUBRR FOSC/16/BAUD-1
volatile char data=0, sp, N=1;//N - будет переключателем скорости 1,2,3. Работаю над этим
volatile bool i=0;
void USART_Init( unsigned int ubrr)
{
	//Set baud rate
	UBRR0H = (unsigned char)(ubrr>>8);
	UBRR0L = (unsigned char)ubrr;
	//Enable Receiver
	UCSR0B |= (1<<RXEN0);
	// Разрешение прерываний по приему
	UCSR0B |= (1<<RXCIE0);
	// Set frame format: 8data, Odd Parity, 2stop bit
	UCSR0C|= (1<<UCSZ00)|(1<<UCSZ01)|(1<<UPM00)|(1<<UPM01)|(1<<USBS0);
}
ISR (USART_RX_vect)
{
	data=UDR0;
}
void timer0_init (void)
{
//Normal port operation, Normal
	TCCR0B=(1<<CS01);//
	OCR0A=150;//
	TIMSK0|=(1<<OCIE0A)|(1<<TOIE0);//Timer/Counter0 Output Compare Match A Interrupt Enable, Overflow Interrupt Enable		
}
ISR (TIMER0_COMPA_vect)
{
	if ((PORTB&0b00001100)==0b00001100)
	{
		PORTB=PORTB&0b11110011;
	}
}
ISR (TIMER0_OVF_vect)
{
	if (i==1)
	{
		PORTB=PORTB|0b00001100;
	}
	else
	{
		PORTB=PORTB&0b11110011;
	}
}

void timer1_init (void)
{
	TCCR1B|=(1<<WGM12);//CTC, Normal port operation, OC1A/OC1B disconnected.
	TCCR1B|=(1<<CS10)|(1<<CS11);//prescaling 64
	OCR1A=0xffff;//TOP
	TIMSK1|=(1<<OCIE1A);//Output Compare A Match Interrupt Enable
}
ISR (TIMER1_COMPA_vect)
{//если долго нет сигнала от пульта все данные останавливаем моторы
	if ((UCSR0A&0b10000000)==0)
	{
		PORTB=0x00;
		i=0;
	}
}

int main(void)
{
	DDRB=0xff;
	PORTB=0x00;
	DDRD=0xff;
	PORTD=0x00;
	timer0_init();
	timer1_init();
	USART_Init(MYUBRR);
	sei();
	while(1)
	{
		cli();
		if (data!=0)
		{	
			if ((data & (1<<5)) == 0)//кнопка не нажата
			{
				switch (data>>6)
				{
					case 0b01:
					PORTB=0b00001100;i=1;
					OCR0A=(data&0b00011111)*N+160; //160 чтобы мотор стронулся с места
					break;
					case 0b00:
					PORTB=0b00111100;i=1;
					OCR0A=(data&0b00011111)*N+160;
					break;
					case 0b11:
					PORTB=0b00101100;i=1;
					OCR0A=(data&0b00011111)*N+160;
					break;
					case 0b10:
					PORTB=0b00011100;i=1;
					OCR0A=(data&0b00011111)*N+160;
					break;
				}
			}
			
			if ((data & (1<<5)) != 0)//кнопка нажата
			{
				switch (data&0b00000111)//здесь просто звук нажатия на клавишу, ничего более
				{
				case 0b001:
				PORTD=0xff;
				_delay_ms(100);
				PORTD=0x00;
				break;
				case 0b010:
				PORTD=0xff;
				_delay_ms(100);
				PORTD=0x00;
				PORTD=0xff;
				_delay_ms(100);
				PORTD=0x00;
				break;						
				case 0b111:
				for (int i=0; i<3; i++)
				{
					PORTD=0xff;
					_delay_ms(100);
					PORTD=0x00;
					_delay_ms(100);
				}
				break;
				case 0b110:
				for (int i=0; i<4; i++)
				{
					PORTD=0xff;
					_delay_ms(100);
					PORTD=0x00;
					_delay_ms(100);
				}
				break;
				}
			}				
		data=0;
		}
	sei();
	}
}