Arduino.ru

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

А моя машина упорно лезет на стенку и эхолот не работает.

В чем ошибка?

#define Trig 4  // 4-й вывод Ардуино подключен к выводу Trig HC-SR04
#define Echo 5  // 5-й вывод Ардуино подключен к выводу Echo HC-SR04
#define Trig1 6  // 4-й вывод Ардуино подключен к выводу Trig HC-SR04
#define Echo1 7                    // 5-й вывод Ардуино подключен к выводу Echo HC-SR04

//motor A
//int LF = 13;  // 13-й и 12 выводы Ардуино контролируют направление вращения
//int LB = 12;  // двигателя А
int LF= 13; // player 1 left forward
int LB= 12; // player 1 left back
int RF= 11; // player 1 right forward
int RB= 10; // player 1 right back
//motor B
//int RF = 11;  // Аналогично для мотора В
//int RB = 8;
//int speedPinB = 9;
 
void setup()
{
//pinMode(3, INPUT_PULLUP); // 3-й и 2-й выводы Ардуино назначаются входными,
//pinMode(2, INPUT_PULLUP); // подтянутыми к высокому уровню
  pinMode(LF, OUTPUT);
  pinMode(LB, OUTPUT);
  pinMode(RF, OUTPUT);
  pinMode(RB, OUTPUT);

  pinMode(Trig, OUTPUT); //инициируем как выход
  pinMode(Echo, INPUT); //инициируем как вход
 
  Serial.begin(9600);
  /* задаем скорость общения. В нашем случае с компьютером */
}
unsigned int impulseTime=0;
unsigned int distance_sm=0;
unsigned int impulseTime1=0;
unsigned int distance_sm1=0;
unsigned int a=0;
unsigned int a1=0;
unsigned int b=0;
unsigned int b1=0;
unsigned int c=0;
unsigned int c1=0;
unsigned int cc=0;

void loop()
{
  aa:
  digitalWrite(Trig, HIGH);
  digitalWrite(Trig1, HIGH);
  /* Подаем импульс на вход trig дальномера */
  delayMicroseconds(10); // равный 10 микросекундам
  digitalWrite(Trig, LOW); // Отключаем
  digitalWrite(Trig1, LOW);
  impulseTime=pulseIn(Echo, HIGH); // Замеряем длину импульса
  impulseTime1=pulseIn(Echo1, HIGH);
  distance_sm=impulseTime/58; // Пересчитываем в сантиметры
  distance_sm1=impulseTime1/58;
  Serial.println(distance_sm); // Выводим на порт
  Serial.println(distance_sm1);
  //if (distance_sm<15 || distance_sm1<15) // Если расстояние менее 15 сантиметров
  if (distance_sm<15)
  {
//Стоп 0.5с
digitalWrite (LF, LOW);
digitalWrite (LB, LOW);
digitalWrite (RF, LOW);
digitalWrite (RB, LOW);
delay(500);
if(cc==0)
{
//Поворот вправо
digitalWrite (LF, LOW);
digitalWrite (LB, HIGH);
digitalWrite (RF, HIGH);
digitalWrite (RB, LOW);
delay(500);cc=1; goto ee;
}
if(cc==1)
{
//Поворот влево
digitalWrite (LF, HIGH);
digitalWrite (LB, LOW);
digitalWrite (RF, LOW);
digitalWrite (RB, HIGH);
delay(500);cc=0;
}
ee: ;
//Стоп 0.5с
digitalWrite (LF, LOW);
digitalWrite (LB, LOW);
digitalWrite (RF, LOW);
digitalWrite (RB, LOW);
delay(150);
}
else
{
digitalWrite (LF, HIGH);
digitalWrite (LB, LOW);
digitalWrite (RF, HIGH);
digitalWrite (RB, LOW);
}
goto aa;
}