Arduino.ru

Здравствуйте, я пытаюсь собрать своего первого робота на arduino uno, но не могу решить одну проблему.

На передней части робота находится серва с установленным на нее сонаром. При работе программы сонар измеряет расстояние перед роботом и если оно меньше 50см. сонар "оглядывается" на 180 градусов, и заносит данные в массив, после чего он должен вернуться в исходное состояние, но этого не происходит. Программа залипает на цикле. Пробовал использовать циклы for и while, for выполнялся и программа останавливалась, а while зависал при активации...
Может кто объяснить нубу в чем косяк и как идею реальзовать?

#include <Servo.h>//Загрузка библиотеки сервы
Servo servoMain; // Инициализация сервы

#define D1 2          // Направление вращение двигателя 1
#define M1 3          // ШИМ вывод для управления двигателем 1
#define D2 4          // Направление вращение двигателя 2
#define M2 5          // ШИМ вывод для управления двигателем 2

int x = 0;
int myArray[36];
int i = 104; //Переменная отвечающая за положение сервы
boolean trig = false; //Направление вращения
bool direction = 0;   // Текущее направление вращения       
int value;            // Текущее значение ШИМ
const int Trig = 8; 
const int Echo = 9; 

void setup() 
{ 
pinMode(Trig, OUTPUT); 
pinMode(Echo, INPUT); 
pinMode(D1, OUTPUT);
pinMode(D2, OUTPUT); 
servoMain.attach(10); // Сарва присоединена к 10 выводу  
Serial.begin(9600); 
}

unsigned int time_us=0;
unsigned int distance_sm=0;

 void loop() 
{ 
   servoMain.write(i);
   dist();
   Serial.println(distance_sm); // Выводим на порт
   if (distance_sm<50) // Если расстояние менее 50 сантиметром 
{ 
   scan();
}
  servoMain.write(104);
}

 void forward()
 {
digitalWrite(D1, 0);  // Задаем направление вращения
analogWrite(M1, 200);       // Задаем скорость вращения
 }
 
 void scan()
 {
   Serial.println(x);
   
 do
 {
   i=x*5;
   servoMain.write(i);
   dist();
   myArray[x]=distance_sm;
   Serial.print(myArray[x]);
   Serial.print(" ");
   Serial.println(i);
   Serial.println(distance_sm);
   Serial.println(x);
   x=x+1;
   
 } while (x <= 36);

 }
 
 void dist()
 {
   digitalWrite(Trig, HIGH); // Подаем сигнал на выход микроконтроллера 
   delayMicroseconds(10); // Удерживаем 10 микросекунд 
   digitalWrite(Trig, LOW); // Затем убираем 
   time_us=pulseIn(Echo, HIGH); // Замеряем длину импульса 
   distance_sm=time_us/58; // Пересчитываем в сантиметры
   delay(150);
 }