Własny program sterujący

Witam
Jestem w trakcie pisania programu sterującego manipulatorem typu scara(2 osie obrotowe, 1 przesuwna). Na razie program odczytuje plik z g-codem i w zrobionej symulacji ramię robota porusza się tylko do punktów opisanych w g-codzie. I stąd moje pytanie - jak zrobiliście to w Waszych programach, że efektor,np wrzeciono pojedzie po odpowiedniej trajektorii?? Czy zrobiliście to na tej zasadzie, że obliczyliście przyrost odległości w osi X i Y, i na podstawie stosunku tych przyrostów, obliczany jest stosunek prędkości w 1 i w 2 osi??
Pozdrawiam

Znajdź algorytmy Bresenhama.