Arduino.ru

По моей программе робот должен доезжать до ближайшейго препятствия и объехать вокруг него. Я установил на робота серво вместе с ультразвуковым датчиком расстояния. Для начала я сделал скетч с циклом, в котором робот должен был ехать вперед, пока расстояние будет больше определенного. Тогда все работало нормально. Потом я дописал этот скетч, после чего робот стал как бы дергаться, и вал серво тоже дрожит. При этом робот периодически двигается вперед, но сразу же останавливается. вот текст программы:

#define M11 5
#define M12 6
#define M21 11
#define M22 10
#include <Ultrasonic.h>
#include <Servo.h>

float dist, rast;                    //Функции для измерения расстояния и положения серво
Ultrasonic dalnomer(12, 13);       
Servo MyServo;

void setup() {
  MyServo.attach(9);
  pinMode(M11, OUTPUT);              //Контакты для двигателей
  pinMode(M12, OUTPUT);
  pinMode(M21, OUTPUT);
  pinMode(M22, OUTPUT);  
}

void loop() {
  MyServo.write(90);                 //Серво смотрит прямо
  
  do{
    dist = dalnomer.read(CM);        //Считываем расстояние с дальномера
    goPered(255);                    //Едем вперед
  }while(dist>5);                    //Пока расстояние больше 5 см
  goStop();                          //Остановка
  delay(1000);                       //Задержка

                                      //Если убрать код начиная отсюда
  MyServo.write(180);                 //Поворачиваем серво налево
  dist = dalnomer.read(CM);           //Считываем расстояние с дальномера
  if(dist>30){                        //Проверка на отсутствие стенки
    do{
      dist = dalnomer.read(CM);
      goPered(255);                  
    }while(dist>10);                  //Езда вперед
  }
  else{                               //То же самое, только в правую сторону
    MyServo.write(180);
    dist = dalnomer.read(CM);
    if(dist>30){
      do{
        dist = dalnomer.read(CM);
        goPered(255);    
      }while(dist>10);
    }
  }

  rast = MyServo.read();            //На какой угол повернут серво?

  if(rast==0){                      //Если налево, то
    MyServo.write(180);             //поворачиваем направо
dist = dalnomer.read(CM);       //проверка на отсутствие стенки
    if(dist>30){                    //если нет, то
      goPravo(255); delay(1000);    //Поворачиваем направо
      MyServo.write(90);            //Серво смотрит прямо
    }
  }

  if(rast==180){                    //Симметричная ситуация
     MyServo.write(0);
     dist = dalnomer.read(CM);
     if(dist>30){
       goLevo(255); delay(1000);
       MyServo.write(90);
     }
  }
}                                    //до сюда, то робот выполняет то, что было в начале функции loop, как планировалось
				     //то есть едет вперед до ближайшего препятствия. По плану, если присоединить код который я дописал,
				     //он должен проверить если возможность повернуть, и после этого обогнуть препятствие. Но вместо этого,
				     //он даже не двигается с места

int goPered(int Skorost){            //Движение вперед
  analogWrite(M12, LOW);
  analogWrite(M21, LOW);
  analogWrite(M11, Skorost);
  analogWrite(M22, Skorost);}

int goNazad(int Skorost){            //Движение назад
  analogWrite(M11, LOW);
  analogWrite(M22, LOW);
  analogWrite(M12, Skorost);
  analogWrite(M21, Skorost);}

int goLevo(int Skorost){             //Поворот налево
  analogWrite(M12, LOW);
  analogWrite(M22, LOW);
  analogWrite(M11, Skorost);
  analogWrite(M21, LOW);}

int goPravo(int Skorost){            //Поворот направо
  analogWrite(M11, LOW);
  analogWrite(M21, LOW);
  analogWrite(M12, Skorost);
  analogWrite(M22, LOW);}

void goStop(){                       //Остановка
  digitalWrite(M12, LOW);
  digitalWrite(M11, LOW);
  digitalWrite(M21, LOW);
  digitalWrite(M22, LOW);}
      MyServo.write(90);
    }
  }

Срочно нужна помощь, если дадите совет - буду благодарен

P.S. Драйвер двигателя - мостовой, на основе микросхемы L293;

P.P.S. До проверки всей программы, робот доезжал до стенки нормально. теперь и этот кусок кода не работает(Я ничего в нем не менял)