Hast auch überprüft, ob die gefahrene Bahn mit der Berechneten übereinstimmt?
Man sieht ganz gut ob man richtig gerechnet hat, wenn man mit der Fußspitze einfach mal eine Gerade abfährt, also nur eine Koordinate über einen bestimmten Bereich inkrementiert.

Gruß
Malte