Dein Fehler ist MotorSpeed(c, d); muss in die while-Schleife, da diese nich verlassen wird. Sonst fallen mir keine Fehler auf.

mfg liggi