Myslím, že dělá, kroky je potřeba rozdělit tak, aby s počtem pulsů z enkodéru ("časem") rostla frekvence (nikoliv perioda, což je trochu zrada) lineárně (příp. ještě lépe po S křivce, ale to je asi dost masakr) a co nejplynuleji. Inspirovat by se možná dalo tady, metoda make_pulses (Stepping Type 0). Asi by mělo jít vygooglit i něco srozumitelnějšího.
EDIT: Ještě mě napadly GRBL zdrojáky.