Cześć ,

robię profiler prędkości(PID) do robota dwukołowego. Mam problem z wyliczeniem przejechanej trasy, tak aby robot przejechał dany kawałek po PROSTEJ i się zatrzymał. Umieszczam na dole kod z zaznaczonym,i miejscami gdzie mam problem. Wiem mniej więcej co ma tam być ale robot nie działa poprawnie. Czy byłby ktoś wstanie mi pomóc w kwestii prędkości/trasy do tego kodu ?

Silniki są sterowane PWM
Średnica kół to 3.22 cm.
Rozstaw kół to 90 mm.
Ilość "tików" enkodera na obrót koła to 300



Wiem, że jest ogromny chaos z zmiennymi i tym gdzie powinny być(globalne/lokalne, rozmiar ich), ale to już sobie poprawię jak całość będzie działać
Kod: