Hallo Chrise,
dein Programm rechnet sich meiner Meinung nach den 'Wolf' für nix und wieder nix.

Du nutzt einmal vor der while-Schleife ein MotorDir(), aber deine Rechenergebnisse (Variablen c und d?) werden nirgendwo benutzt um mit MotorSpeed() irgendwo eine Wirkung zu erzielen.
Habe ich da nun etwas total übersehen, oder ist dein geposteter Programmcode nur ein Ausschnitt?

Gruß Sternthaler