Заставить мои 2 мотора (с колесами) повернуть налево, чтобы он отслеживал левую сторону с помощью датчика дальности

Итак, чтобы перейти прямо к этому, у меня сейчас есть настройка на использование лазерного датчика urg, arduino pro mini (slave), raspberry pi (master) и Sabertooth 2×12 PWM, управляющих 2 двигателями.

кстати я программирую в cpp

Я все настроил, и используя данные лазерного датчика, которые я получаю от лазерного датчика urg, я могу получить переднюю, левую, правую, переднюю часть, переднюю правую, заднюю левую и заднюю часть моей маленькой системы, которая управляется 2 колеса привода (мотор) и 2 переменные колеса спереди.

Я слежу за устаревшей стеной, и предполагается, что она поворачивает налево, когда датчик заднего хода ничего не обнаруживает.

Это идет отлично! пока он не совершит поворот (поворот будет установлен на 45, когда он обнаружит, чтобы сделать разворот вокруг устаревшей стены) и остановится на полпути, потому что задний левый обнаруживает стену еще на половине поворота. Это трудно объяснить, если вы не визуализируете это.

который я попробую через аски

--------------------
---->           [] <-- my device (backleft detects nothing at this point)

середина поворота

-------------------- [] <-- it now detects a backleft and stops

в любом случае, чтобы сделать это сделать плавный левый поворот без задержки. Я хочу сделать это как можно более плавным. Уже добавлен PID, так что это последняя часть 🙁

Спасибо!!