Самодельный ИК-пульт для iRobot Roomba

/*
Пульт управление роботом-пылесосом Irobot Roomba. Программа для контроллера Atmel ATTINY2313A.
FUSE-биты:

Fuse Low Byte:
CKDIV|CKOUT|SUT|SKSEL|
  0  |  1  |10 |0100 | 0x64 Default
  1  |  1  |10 |1100 | 0b11101100=0xEC  для кварца 3-8 МГц
*/

//#define F_CPU 7.3728E6
//#define __AVR_ATtiny2313A__

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>

/*------<Макросы>-----*/
//Частота 39477 Гц
//Период 25,3 мкс:
#define P_GEN {TCNT0=0; PORTB |= 0x8; while (TCNT0 < 93); PORTB &= ~0x8; while (TCNT0 < 186);}

//Roomba коды:
#define LEFT 			129
#define FORWARD 		130
#define RIGHT 			131
#define SPOT 			132
#define DOCK 			133
#define CLEAN 			136
#define PAUSE 			137
#define POWER 			138
#define FORWARD_LEFT  	139
#define FORWARD_RIGHT 	140
#define TEST 			170


/*----------------<Функции:>----------------*/
void init(void)
{
//Установка делителя частоты на /1:
	CLKPR = (1<<CLKPCE);
	CLKPR = (0<<CLKPS0)|(0<<CLKPS1)|(0<<CLKPS2)|(0<<CLKPS3);
//Настройка портов:
	PORTD = 0xFF;
	DDRD  = 0x0;
	PORTB = (1<<PORTB0)|(1<<PORTB1)|(1<<PORTB2);
	DDRB  = (1<<DDB3)|(1<<DDB4);
//Настройка прерываний:
	GIMSK = 0x30; //(1<<PCIE)|(1<<4);
	PCMSK = 0x3; //(1<<PCINT0)|(1<<PCINT1);
	PCMSK2 = 0x7F;
}

void ir_tx(unsigned char data_ir)
{
	PORTB |= (1<<PB4); //Включим светодиод
/*
Кодирование данных:
1:
__________
2862uS    |988uS
          |_____
0:
_____
887uS|  2862uS
     |__________
В конце пауза ~16870 uS
*/
	unsigned char cnt;
//Настраиваем таймеры:
	TCCR0B = (0<<CS12)|(0<<CS11)|(1<<CS10); // Для несущей f/1
	TCCR1B = (0<<CS12)|(1<<CS11)|(1<<CS10); // Для данных f/64	
        for (cnt=0; cnt<8; cnt++)
        {
            TCNT1=0;
            if (data_ir & (0x80 >> cnt))
            {	//Если 1
                while (TCNT1 < 330) P_GEN;
                while (TCNT1 < (330+114)) ;
            }
            else
            {	//Если 0
                while (TCNT1 < 113) P_GEN;
                while (TCNT1 < (330+114)) ;
            }
        }
        TCNT1=0;
        while (TCNT1 < 1950)  ;   
	PORTB &= ~(1<<PB4);	//Выключим светодиод
}

void send_command(void)
{
	volatile unsigned char button_b;
	volatile unsigned char button_d;
	volatile unsigned char button_b_new;
	volatile unsigned char button_d_new;
	unsigned char pause_cnt = 0;
	TCCR1B = (0<<CS12)|(1<<CS11)|(1<<CS10); // f/64
	TCNT1=0;
	while (TCNT1 < 346); //346 - Задержка 3 ms

	button_b = ~PINB & ((1<<PINB0)|(1<<PINB1));
	button_d = ~PIND & 0x7F;
	button_b_new = button_b;
	button_d_new = button_d;
	
    while ((button_b == button_b_new)&&(button_d == button_d_new))
    {
        if ((button_b_new == 0) && (button_d_new == 0)) break;		
		if (button_b == (1<<PINB0)) {ir_tx(DOCK);}
        if ((pause_cnt < 20) && (button_b == (1<<PINB1))) {ir_tx(PAUSE); pause_cnt++;}
		if (pause_cnt >= 20) {ir_tx(POWER);}
        if (button_d == (1<<PIND0)) {ir_tx(FORWARD_LEFT);}
        if (button_d == (1<<PIND1)) {ir_tx(CLEAN);}
        if (button_d == (1<<PIND2)) {ir_tx(LEFT);}
        if (button_d == (1<<PIND3)) {ir_tx(FORWARD);}
        if (button_d == (1<<PIND4)) {ir_tx(SPOT);}
        if (button_d == (1<<PIND5)) {ir_tx(RIGHT);}
        if (button_d == (1<<PIND6)) {ir_tx(FORWARD_RIGHT);}
		button_b_new = ~PINB & ((1<<PINB0)|(1<<PINB1));
        button_d_new = ~PIND & 0x7F;
		
    }
}

/*----------------------<Обработка прерываний:>------------------------*/
ISR(PCINT_B_vect)
{
	cli();
	send_command();
	sei();
}

ISR(PCINT_D_vect)
{
	cli();
	send_command();
	sei();
}

/*-----------------<основной цикл программы>-----------------------*/
int main(void)
{
	init();
	sei();
	for(;;)
	{
		set_sleep_mode(SLEEP_MODE_PWR_DOWN);
		sleep_mode();
	}
}