[ARDUINO] sterowanie silnikiem krokowym

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#define SILNIK_DDR DDRB
#define SILNIK_PORT PORTB
#define SILNIK_PIN PINB
#define SILNIK_F1 1 /*LSB*/
#define SILNIK_MASK (0x0f<<SILNIK_F1)

#define PAUSE() ({ dir = pause; })

#define BAUDRATE 19200UL
#define PRESCALE (((F_CPU / (BAUDRATE * 16UL))) - 1)

enum Mode {pause, const_left, const_right, left, right, set_upper, set_lower};
volatile uint8_t dir = 0;
volatile uint16_t lowBound = 0;
volatile uint16_t upBound = 2000;

ISR (TIMER0_COMPA_vect)
{
    const uint8_t tab[] = {1,3,2,6,4,12,8,9};
    static int16_t i = 0;
    SILNIK_PORT &= ~(SILNIK_MASK);
    if (dir && (i > lowBound) && (i < upBound)) {
        SILNIK_PORT |= (tab[i % 8] << SILNIK_F1);
    }

    if (dir == const_left && i < upBound) {
        i++;
    } else if (dir == const_right && i > lowBound) {
        i--;
    }
    if (dir == left) {
        i++; PAUSE();
    }
    if (dir == right) {
        i--; PAUSE();
    }
    if (dir == set_upper){
        PAUSE();
        upBound = i;
    }
    if (dir == set_lower){
        PAUSE();
        lowBound = i;
    }
}

int main (void)
{
    uint8_t cmd;
    SILNIK_DDR |= SILNIK_MASK;

    TCCR0B |= (1<<CS02);   //preskaler
    TIMSK0 |= (1<<OCIE0A); //Wlacz przerw. dla Timera0
    TCCR0A |= (1<<WGM01);
    UCSR0B |= (1<<RXEN0)|(1<<TXEN0);
    UBRR0 = PRESCALE;
    OCR0A = 100;
    sei();

    for(;;) {
        while ( !(UCSR0A & (1 << RXC0)) );
        cmd = (int)UDR0;
        switch(cmd)
        {
            case 108: // l
                dir = const_left;
            break;
            case 112: // p
                dir = const_right;
            break;
            case 52: // 4
                dir = left;
            break;
            case 54: // 6
                dir = right;
            break;
            case 100: // d
                dir = set_lower;
            break;
            case 103: // g
                dir = set_upper;
            break;
            default:
                dir = pause;
        }
    }
    return 0;
}