Ich hätte auch als ersten Test die verschiedenen Verzögerungen (Delay) deaktiviert.
Falls du auch nicht sicher bist ob die Sensoren richtige Werte liefern, würde ich zuerst ein Test-Sketch schreiben und die Analogwerte prüfen.
Anschliessend würde ich jede einzelne Funktion einzeln austesten. Einfach systematisch vorgehen und nicht zu viel aufs Mal testen wollen.
Je mehr ungeprüfter Code vorhanden ist, umso mehr Fehler können sich darin befinden.
Wenn die einzelnen Funktion (Vorwärts, Rückwerts, Halt) wie erwartet laufen, kannst du diese zum gesamten Programm zusammenfügen.
Viel Erfolg.