Aber wie kann ich das mit dem Winkel regeln? Weiter kann die die Abstandsänderung zur Wand berechnen in dem ich den alten Abstand zum neuen Abstand subtrahiere und druch die Messzeit dividiere. Aber wie komme ich nun weiter?
Das klingt ja schon irgendwie kompliziert.

Die wohl einfachste Methode wäre:
Der Lenkwinkel ist proportional zur Differenz aus dem aktuellen Abstand und dem gewünschten Abstand (Einfacher P-Regler).

Du hast ja jetzt auch einen Regelmechanismus, wie funktioniert der?
Kann man da nicht einfach den Sollwert ändern?