Nie do końca działający sterownik silnika krokowego ....

Witam
Chciałem zbudować na razie w celach doświadczalno naukowych sterownik do silnika krokowego na procesorze Attiny2313, taktowany zewnętrznym kwarcem 4MHz. Silnik krokowy mam z drukarki HP ma on 48 kroków na obrót, i 6 przewodów ( z tego co znalazłem jest to typowy silnik krokowy ).
Napisałem program, który zamieszczam poniżej ( w BASCOM-ie ).
I teraz tak po uruchomieniu programu na wyświetlaczu pojawia się informacja o aktualnym stanie sterownika, jednak przez kilka pierwszych sekund od uruchomienia procesora nie można zmienić żadnego parametru, następnie parametry "odblokowują" się, można je zmienić, ale nie działa silnik, dopiero po jakiś 10 sekundach silnik zaczyna pracować (tutaj dodam, że sprawdzałem prędkość obrotową i zgadza się ona z zadaną ). Dodatkowo jeżeli silnik chodzi to chodzi ale jak zaczynam go przyśpieszać to po zmianie prędkości np. z 10 obr/min na 50 obr/min, silnik sam się zatrzymuje na jakieś 10 sek i później dalej rusza z ustawioną prędkością ( tutaj 50 obr/min )
Ps. pierwszy tryb to praca silnika z dokładnością 96 kroków na obrót, drugi tryb to 48 kroków na obrót, ten tryb niestety nie działa wcale.

Tak na pierwsze wrażenie:
Sterowanie silnikiem powinno odbywać się w pętli głównej-jest to główne zadanie programu.
W przerwaniach należy zmieniać tylko parametry sterowania.
Muszą się wykonywać szybko bo zakłócają sterowanie silnika.

Ok to przerobię, a co do błędu to już znalazłem, jeden licznik wybiegał poza narzucony zakres, a że miał wartość word to blokował procek na kilkadziesiąt nawet sekund.