Робот объезжающий препятствия пользуясь ультразвуковым датчиком расстояния - USonic Rider

Продолжаем экспериментировать с гусеничной платформой от Pololu.
На этот раз едем куда попало в автономном режиме, но объезжаем препятствия используя ультразвуковой датчик расстояния. А раз автономно, то можем с чистой совестью считать эту машинку роботом.
Машинка состоит из (слева направо):

• Blockduino
• BlockMotor
• PlugDIO
• Ultrasonic Range Detector

Ультрасоник подключил в свободные порта: пин Trig в порт D11, а пин Echo в порт D10.

Снова решил не изобретать велосипед и взял первый попавшийся алгоритм, найденный на сайте http://cyber-place.ru.

Алгоритм работы прост:

• замеряем расстояние
• если расстояние меньше 10 см, то чуток поворачиваем в заданную сторону,
• иначе если расстояние меньше 20 см, то поворачиваем уже чуть больше,
• иначе если расстояние меньше 40 см, то поворачиваем совсем больше,
• иначе считаем что путь свободен и едем вперед и меняем следующее направление поворота
• начинаем все сначала;

Как ультрасоник замеряет  расстояние описывать не буду, таких описаний много на просторах интернета, просто его работу вынес в отдельную подпрограмму LookAhead(), что можно перевести как: Посмотреть Вперед. :)

Код под спойлером:

// UltraSonic Rider - по алгоритму "Робот объезжающий препятствия" сайта cyder-place.ru
// http://blockduino.org
// http://blockduino.blogger.com/2013/08/usonic-rider.html
// Август 2013

#include <BDMotor.h>  //для BlockDuino и BlockMotor
//#include <AFMotor.h>  //для Arduino и MotorShield

#define SPEED 255        // скорость вращения моторчиков, 255 - максимум

#define ECHO_PIN 10      // номер порта для ножки Echo
#define PING_PIN 11      // номер порта для ножки Ping (или Trig)

boolean isCW = true;     // переменная для хранения направления поворота 
double dist;             // переменная для хранения дистанции
AF_DCMotor mL(1);        // #1 моторчик левой гусеницы
AF_DCMotor mR(2);        // #2 моторчик правой гусеницы


void setup() {
  pinMode(ECHO_PIN, INPUT); 
  pinMode(PING_PIN, OUTPUT);
  digitalWrite(PING_PIN, LOW);
  
  mL.setSpeed(SPEED);    // устанавливаем скорость вращения левого моторчика
  mR.setSpeed(SPEED);    // устанавливаем скорость вращения правого моторчика
}

void loop() { // Основное тело программы
  dist = LookAhead();                    // замеряем дистанцию
  if(dist < 10) Rotation(2000);         // если меньше 10 см  максимальный угол разворота 
    else if(dist < 20) Rotation(1000);  // иначе если меньше 20 см  средний угол разворота 
      else if(dist < 40) Rotation(500); // иначе если меньше 40 см  минимальный угол разворота 
         else {
                Run();                   // если пределы не достигнуты, то едем вперед
                isCW=!isCW;              //   и меняем направление следующего поворота
              }
}             // конец основной программы, начинаем все сначала

//
// Подпрограммы
//
double LookAhead(){ // подпрограмма замера дистанции
  double duration;
  
  digitalWrite(PING_PIN, HIGH);  // дергаем ножку на UltraSonic
  delayMicroseconds(10);         //
  digitalWrite(PING_PIN, LOW);   //
  
  duration = pulseIn(ECHO_PIN, HIGH)/ 58.824; // Замеряем время импулься и переводим в расстояние в сантиметры
  if (duration > 140) duration = 140;         // Отсекаем ложные срабатывания
  return duration;
}

void Rotation(int t){  // подпрограмма поворота
  if (isCW) TurnRight();   // если переменная isCW правда (true) то поворачиваем направо, 
    else TurnLeft();       //   иначе налево
  delay(t);                // ждем нужное время
  Stop();                  // останавливаемся
}

void Stop(){  // подпрограмма Остановка
      mL.run(RELEASE);
      mR.run(RELEASE);
}

void Run(){  // подпрограмма Ход вперед
      mL.run(FORWARD);
      mR.run(FORWARD);
}

void TurnRight(){ // подпрограмма Поворот направо
      mL.run(FORWARD);
      mR.run(BACKWARD);
}

void TurnLeft(){ // подпрограмма Поворот налево
      mR.run(FORWARD);
      mL.run(BACKWARD);
}

Хотелось бы осветить и недостатки такого робота:

1. Ширина звукового луча сенсора заметно уже ширины шасси, поэтому есть шанс что робот въедет гусеницей в какой-то угол или ножку стула
2. Звук плохо отражается от пушистых поверхностей, например кот будет для робота невидим
3. Звук отразится от поверхностей находящихся под острым углом к оси робота не обратно, а уйдет в другую сторону, там погуляет и вернется с ложными данными, порой до нескольких метров место десятка сантиметров. Т.е. в нашем случае эти поверхности тоже невидимы.

Вывод: Ультрасоник вещь хорошая, но для робота нужны еще и другие датчики, например бампер или инфракрасные детекторы объектов.

Небольшой видео отчет:

Дополнительно: Код расположенный на сайте Сodebender.cc