Stepper motor closed loop control algorithm

(дал заголовок на английском, т.к. сколько ни гуглил по этой теме, ничего толкового не попадалось).
Возможно (скорей всего), какие-то неровности еще остались, но в основном управление уже работает. Функционал с кнопками и концевиками уже в железе буду отрабатывать. Часть с CAN вообще не тестировал еще + не брался за I2C, ну и надо бы одновременно три шаговика погонять: интересно (правда, жалко разъемы тратить, ну да ладно, их есть у меня).
( tl;dr )