Кстати, в алгоритме работы есть серьезный баг. Алгоритм такой - во время старта поршня от НМТ до ВМТ запускается таймер, и контролируется время взведения поршня до ВМТ. Далее программа циклится, и ждет пока поршень пройдет ВМТ.
Так вот, если поршень застрянет в ВМТ (н-р слабый аккум, проблема в моторе) то программа зависнет с поданным на заклиненный мотор напряжением. Со всеми вытекающими.
Выход - контролировать таймер взвода до момента прохождения ВМТ, а не до его начала.
Процедура называется VMT2.