Hallo,

zu deinem vorigen Code ein paar Anmerkungen.
* Die Init und InitLCD sollten nur einmal beim Programmstart aufgerufen werden, nicht innerhalb der while Schleife.
* Innerhalb der for Warte Schleifen werden MotorSpeed und PrintLCD gemacht. Die brauchen ebenfalls nur einmal außerhalb der Warteschleifen aufgerufen werden.

Dein anderes Programm werde ich bei Gelegenheit mal ausprobieren. Leider hat mich meine Frau derzeit auf Roboter Entzug gesetzt.