Arduino.ru

Здравствуйте товарищи форумчане! С Ардуино я новичок и это мой первый проетк... Проект представляет из сетбя самодельную основу, Ардуино "дуемиланов", 4 серво ( http://arduino-direct.com/sunshop/index.php?l=product_detail&p=126 , обединённые по 2-ум боковым серво с 1 контроллером), дальномер и серво(на 130 градусов), поворачивающий дальномер по сторонам. Остаются кое-какие нюансы с кодом:

// Библиотеки
#include <Stepper.h>
#include <Ultrasonic.h>
#include <Servo.h>
#define STEPS 100

//Ввод переменных
Ultrasonic ultrasonic(3, 2);
Stepper stepper_1(STEPS, 10, 12, 11, 13);
Stepper stepper_2(STEPS, 4, 6, 5, 7);
Servo myservo;


int ugol_1;
int ugol_2;
int pos = 65;
int pos_L = 130;
int pos_R = 0;
int dist_L;
int dist_R;
int i = 1;


void setup() 
{
stepper_1.setSpeed(300); //условная ед. скорости серво 
stepper_2.setSpeed(300); 
myservo.attach(9);
}


void loop() 
{ 
// Сам алгоритм
float dist_cm = ultrasonic.Ranging(CM); // дальномер высчитывает расстояние
if (dist_cm > 15) //Авто едет прямо, если расстояние впереди больше 15 см
{ 
ugol_1 = 1; 
ugol_2 = -1; 
stepper_1.step(ugol_1) ; stepper_2.step(ugol_2);
}

if (dist_cm < 16) //Если расстояние перед авто меньше 16 см,
  // серво моторчик должен повернуть дальномер на ~90 градусов влево, потом на столько же в право  

//(сравнивается расстояние)
{ 
myservo.write(pos_L); 
delay(500); 
dist_L = dist_cm; 

myservo.write(pos_R); 
delay(500);
dist_R = dist_cm;
myservo.write(pos); 
}
else 
{
myservo.write(pos);
}




if ((dist_R > dist_L) && (dist_cm < 16)); //если расстояние справа > левого,
// авто поворачивает на 90 градусов направо... аналогично и во второй части
{
for (int i=0; i <= 2350; i++) // 2350 for 90'
{
ugol_1 = -1; 
ugol_2 = -1; 
stepper_1.step(ugol_1); stepper_2.step(ugol_2); // по часовой стрелки
}
} 
if ((dist_R < dist_L) && (dist_cm < 16))
{
for (int i=0; i <= 2350; i++) // 2350 for 90'
{
ugol_1 = 1; 
ugol_2 = 1; 
stepper_1.step(ugol_1); stepper_2.step(ugol_2); // против часовой стрелки
}
} 
}
// по не известной причине вместо того, чтоб сравнить расстояние с обоих сторон
// и повернуться в соответствующую сторону, серво поворачивает дальномер не 1 раз...а безконечность
//(и в итоге не разворачивается авто)

Прошу помочь советом или кодом... Спасибо

P.S. Извините за возможные орфографические и логические ошибки!