Нужна помощь с програмой для робота...

Собрал робота из Arduino UNO + Arduino Motor Shield + серва и ИК-датчиком, пару дней ковыряний и конструкция начала работать Но один момент ни как не могу решить Прошу помощи. Это чудо ездит и даже объезжает препятствия. Но вот когда заезжает в узкое место, то крутиться пока не развернётся. Хотелось, что бы он, уперевшись в препятствие, по алгоритму проверил расстояние по бокам и если он маленькое, выехал задним ходом и нашёл другой маршрут.

Код ( (Unknown Language)):

//==================================================================================
//Прога для робота на гусеницах
//==================================================================================
#include <Servo.h>
//Контакты Motor Shield (DIR_A 12, DIR_B 13, PWM_A 3, PWM_B 11, BRAKE_A 9, BRAKE_B 8)
#define SPEED_LEFT  11
#define SPEED_RIGHT  3
#define DIR_LEFT  13
#define DIR_RIGHT  12
#define BRAKE_LEFT  8
#define BRAKE_RIGHT  9
// Датчик
#define IR_RADAR  A2
// Расстояние в см
float dist(){
  float volts = analogRead(IR_RADAR)*0.0048828125;
  float distance = 65*pow(volts, -1.10);
  return distance;
}
//Препятствие
const int dangerThresh = 50;
int dist_l, dist_r;
// Серво
#include <Servo.h>
#define SERVO  2
Servo radarServo;
//==================================================================================
//Маршрут движения
void CalcMove()
{
 if (dist_l>dist_r) //если слево меньше...
 //...то едем вправо
 {
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, LOW);
  delay(700); //время на манёвр
 }
 else if (dist_r>dist_l) //если справо меньше...
 //...едем влево
 {
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, LOW);
  digitalWrite(DIR_RIGHT, HIGH);
  delay(700); //время на манёвр
 }
 else if (dist_l, dist_r > dangerThresh*2)
 {
 analogWrite(SPEED_LEFT, 75);
 analogWrite(SPEED_RIGHT, 75);
 digitalWrite(DIR_LEFT, LOW);
 digitalWrite(DIR_RIGHT, LOW);
 delay(1000);
 }
}
//==============================================================================
void setup()
{
  // Выходы двигателей  
  pinMode(SPEED_LEFT, OUTPUT);
  pinMode(SPEED_RIGHT, OUTPUT);
  pinMode(DIR_LEFT, OUTPUT);
  pinMode(DIR_RIGHT, OUTPUT);
  // Подключение к серво  
  radarServo.attach(SERVO);
  // Выставить серво по центру (обычно 90)
  radarServo.write(86);
  // Настраиваем порт для мониторинга данных  
  Serial.begin(9600);  
  // Пауза перед началом работы
  delay(4000);
}

//==============================================================================
void loop()
{
  //Вывод данных с датчика в см
  Serial.println(dist());
  //Расчет маршрута объезда препятствия
  int dist_f = dist(); //расстояние спереди
  if (dist_f>dangerThresh) //...если больше заданного вначале
  //едем вперёд
  {
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, LOW);
  digitalWrite(DIR_RIGHT, LOW);
  }
  else //...иначе
  {
  analogWrite(SPEED_LEFT, 0);
  analogWrite(SPEED_RIGHT, 0); //Остановка!
  radarServo.write(10); //Поварачиваем датчик вправо
  delay(500); //время на поворот сервы
  dist_r = dist(); //расстояние справо
  delay(500); //врямя для измерения
  radarServo.write(160);//поворот датчика влево
  delay(700); //время для поворота сервы
  dist_l = dist(); //растояние слево
  delay(500); //замеряем
  radarServo.write(86); //датчик в центр
  delay(100); //небольшая выдержка
  CalcMove(); //получив данные идём расчитывать маршрут объезда ;)
  }

  }

 

Проблема в блоке расчёта маршрута, ели я правильно понимаю...

 

Первое что бросается в глаза - строчка:

Код (Text):

else if (dist_l, dist_r > dangerThresh*2)

эта запись эквивалентна записи:

Код (Text):

else if (dist_r > dangerThresh*2)

а то, что вы видимо хотели написать пишется так:

Код (Text):

else if (dist_l > dangerThresh*2 && dist_r > dangerThresh*2)

(это так, не вникая в суть кода)

 

А в остальном, если так прикинуть - то расстояния по бокам практически никогда не будут одинаковыми, т.е. этот кейс никогда не сработает, поэтому (если я правильно понял и код последнего ифа двигает назад) я написал бы так:

Код (Text):

//Маршрут движения
void CalcMove()
{
    // если по бокам мало места - едем назад
  if (dist_l < dangerThresh*2 && dist_r < dangerThresh*2)
  {
    analogWrite(SPEED_LEFT, 75);
    analogWrite(SPEED_RIGHT, 75);
    digitalWrite(DIR_LEFT, LOW);
    digitalWrite(DIR_RIGHT, LOW);
    delay(1000);
  }
  else if (dist_l>dist_r) //если слева меньше...
//...то едем вправо
{
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, LOW);
  delay(700); //время на манёвр
}
else if (dist_r>dist_l) //если справа меньше...
//...едем влево
{
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, LOW);
  digitalWrite(DIR_RIGHT, HIGH);
  delay(700); //время на манёвр
}
}
 

код поворотов влево и вправо не менял, т.к. сам код не соответствует тому что написано в комментариях, но если работает, то ок))

 

Вы говорите о том, что лево и право перепутаны? Это я в процессе постройки робота перевернул серву
Или ещё где накосячил?

За подсказку ОГРОМНОЕ спасибо!

Теперь едет назад при заданных условиях.

И дописал кусочек, что бы обратно в тупик не заезжал

Код ( (Unknown Language)):

//Маршрут движения
void CalcMove()
{
  // если по бокам мало места - едем назад
  if (dist_l < dangerThresh*2 && dist_r < dangerThresh*2)
  {
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, HIGH);
  delay(2000);
  Tupik(); //идем расчитывать маршрут объезда тупика
  }
  else if (dist_l>dist_r) //если слева меньше...
  //...то едем вправо
  {
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, LOW);
  delay(700); //время на манёвр
  }
  else if (dist_r>dist_l) //если справа меньше...
  //...едем влево
  {
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, LOW);
  digitalWrite(DIR_RIGHT, HIGH);
  delay(700); //время на манёвр
  }
}
//==================================================================================
//Маршрут объезда тупика
void Tupik()
{
  //Расчет маршрута выезда из тупика
  int dist_f = dist(); //расстояние спереди
  if (dist_f>dangerThresh) //...если больше заданного вначале
  //едем вперёд
  {
  analogWrite(SPEED_LEFT, 75);
  analogWrite(SPEED_RIGHT, 75);
  digitalWrite(DIR_LEFT, LOW);
  digitalWrite(DIR_RIGHT, LOW);
  }
  else //...иначе
  {
  //Остановка!
  analogWrite(SPEED_LEFT, 0);
  analogWrite(SPEED_RIGHT, 0);
  radarServo.write(10); //Поварачиваем датчик вправо
  delay(500); //время на поворот сервы
  dist_r = dist(); //расстояние справо
  delay(500); //врямя для измерения
  radarServo.write(160); //поворот датчика влево
  delay(700); //время для поворота сервы
  dist_l = dist(); //растояние слево
  delay(500); //замеряем
  radarServo.write(86); //датчик в центр
  delay(100); //небольшая выдержка
  CalcMove(); //получив данные идём расчитывать маршрут объезда ;)
  }
}
 

 

Если судить по условию, то получается в первом случае:
else if (dist_l>dist_r) // расстояние слева больше расстояния справа, т.е. мы должны ехать туда где больше места, т.е. влево, в комменте написано, что едем вправо
Но если у вас он при этом едет в правильном направлении - значит после того как вы перевернули серву у вас поменялись значения переменных dist_l и dist_r, в итоге код стал правильным (но при этом как бы dist_l - уже обозначает расстояние справа а не слева).

 

Ага.. так и есть, серву перевернул, поправил код, но не как правильно, а как было легче А коменты не правил вовсе... На тот момент эта была не проблема, проблема была в выезде из тупика. А ща всё ок, так что перепишу остальное, точнее приведу в порядок

Ещё раз спасибо за подсказку, очень помогло. Без Вас целый день ковырялся, не мог найти правильного решения.