wieso mußt du die schritte mit einem Controller berechnen? Wenn abstand und entfernung gegeben sind, ist es doch nur eine einmalige berechnung?
Ansonsten:
Warum versuchst du es nicht mit einem Regelverhalten, ähnlich wie der Asura mit einem PD-Regler einer Linie verfolgt. Ist Zwar eine andere Anwendung, aber der Ansatz könnte der selbe sein.
Weil eigentlich regelst du dein Steper auf eine Position