Как сделать робота объезжающего препятствия?

всем добрый день. Нужна помощь по коду. Написала код для робота объезжающего препятствия, запускаю робота, а он у меня странные вещи творит) Подскажите,пожалуйста, что поправить. Сначала как и по программе серва устанавливается на 90 градусов, потом робот едет вперед и независимо от расстояния (даже больше 15, робот останавливается и вертится серва, потом все заново повторяется, такое ощущение что замеры не осуществляются.
вот собственно код:
#include <AFMotor.h>//подключаем бибилиотеку для моста
#include <Servo.h>//подключаем бибилотеку для сервопривода
const int Trig=14;
const int Echo=15;
//моторы к клеммам М1, М2, М3, М4
AF_DCMotor motor1(1);
AF_DCMotor motor2(3);
//Объект для сервопривода
Servo myservo;
//переменные для хранения данных с дальномера
unsigned int time_us=0;
unsigned int distance_sm=0;
unsigned int dist_r=0;//расстояние до ближайшего объекта справа
unsigned int dist_f=0;//расстояние до ближайшего объекта прямо
unsigned int dist_l=0;//расстояние до ближайшего объекта слева

void setup()
{
pinMode(Trig,OUTPUT);
pinMode(Echo,INPUT);
Serial.begin(9600);
myservo.attach(10);
myservo.write(90);//поворачиваем серву в положение 90 градусов при каждом включении(надо чтоб был прямо)
}

void loop()
{
motor1.run(FORWARD);
motor1.setSpeed(255);
dist_f=sonar();
if (dist_f<15)
{
motor1.run(RELEASE);//остановка
myservo.write(0);
delay(1000);
dist_r=sonar();
myservo.write(90);
delay(1000);
dist_f=sonar();
myservo.write(180);
dist_l=sonar();
delay(1000);
if(dist_r>dist_f&&dist_r>dist_l)
{
motor1.run(FORWARD);
motor1.setSpeed(255);
motor2.run(FORWARD);//поворот направо(один двигатель, ханизм от радиоуправляемой машинки, т.е. при движении двигателя вперед - поворот направо, двигатель назад - поворот налево
motor2.setSpeed(255);
}
if(dist_l>dist_r&&dist_l>dist_f)
{
motor1.run(FORWARD);
motor1.setSpeed(255);
motor2.run(BACKWARD);//поворот налево
motor2.setSpeed(255);
}
if (dist_f>dist_r&&dist_f>dist_l)
{
motor1.run(FORWARD);//прямо
motor1.setSpeed(255);
motor2.run(RELEASE);
}
if (dist_l<5&&dist_f<5&&dist_r<5)
{
motor1.run(BACKWARD);//назад
motor1.setSpeed(255);
motor2.run(RELEASE);//выключен
}
}
else
{
motor1.run(FORWARD);//движение вперед
motor1.setSpeed(255);
}
}
int sonar()//алгоритм работы сонара
{
digitalWrite (Trig,HIGH);//подаем сигнал на выход микроконтроллера
delay(10);
digitalWrite(Trig,LOW);//Затем убираем
time_us=pulseIn(Echo,HIGH);//замеряем длину импульса
distance_sm=time_us/58;//пересчитываем в см
Serial.println(distance_sm);//выводим на порт
}

 

все, не надо разобралась!)))