Помогите исправить алгоритм

Вот алгоритм
Начинает движение вперёд.

Едет

Видит препятствие

Останавливается

Активируется сервопривод

Смотрит в лево на 45 градусов.

Если видит препятствие встаёт в исходное положение.

Смотрит в право нет препятствий

Едет на право сервопривод приходит в исходное положение.

Если препядствие с двух сторон то стоит на месте.
Но робот дёргаеться и пытаеться проехать вперёд когда видит препятствие.