Робот, объезжающий препятствия

Этот робот едет вперёд, измеряет расстояние ультразвуковым датчиком HC-SR04 и останавливается перед преградой. Затем сервопривод SG90 поворачивает датчик направо и налево, программа выбирает более свободную сторону, и робот продолжает путь.

Фото реальной сборки: aakash11, Arduino Project Hub. Соединения и питание выполняйте по таблицам ниже.

Результат

Алгоритм состоит из семи действий:

1. измерить расстояние впереди;
2. если впереди больше 25 см — ехать прямо;
3. иначе остановиться;
4. немного отъехать назад;
5. измерить расстояние справа и слева;
6. повернуть в сторону большего свободного пространства;
7. вернуть датчик в центр и повторить цикл.

Проект рассчитан на ровный пол и крупные твёрдые препятствия: коробки, стены и мебель. HC-SR04 может плохо видеть ткань, узкие ножки и поверхность под большим углом, поэтому робот нельзя запускать возле лестницы.

Детали

Деталь	Количество	Точная конфигурация
Arduino Uno R3	1	плата на ATmega328P
L298N	1	двухканальный модуль драйвера
HC-SR04	1	ультразвуковой датчик расстояния
SG90	1	обычный сервопривод 0–180°
Кронштейн HC-SR04 для SG90	1	готовый или изготовленный из плотного картона
2WD-шасси	1	два TT-мотора 3–6 В, колёса и шаровая опора
NiMH AA 1,2 В	6	одинаковые заряженные аккумуляторы
Держатель 6×AA с выключателем	1	около 7,2 В
LM2596	1	понижающий модуль, выход 5,0 В
Провода, макетная плата, стяжки	1 набор	для монтажа

Нужны отвёртка, мультиметр, линейка и коробки для испытательного полигона.

Питание и безопасность

Снимите с L298N перемычки ENA, ENB и 5V-EN. Подключите аккумуляторы к силовому входу L298N и входу LM2596. Выход LM2596 заранее настройте мультиметром на 5,0 В.

6×AA NiMH -> выключатель -> L298N +12V
                         -> LM2596 IN

LM2596 5,0 В -> Arduino 5V
              -> L298N 5V
              -> HC-SR04 VCC
              -> SG90 красный провод

Все GND и коричневый провод SG90 соединены вместе.

Не подключайте SG90 к сигнальному выводу вместо питания. При заклинивании сервопривод потребляет большой ток: если он гудит у механического упора, сразу выключите питание.

Первый запуск проводите с поднятыми колёсами. На полу оставьте свободную площадку не менее 2×2 м. Не запускайте робота на столе и возле лестницы.

Механическая сборка

1. Соберите 2WD-шасси: моторы по бокам, шаровая опора сзади.
2. Закрепите батарейный отсек снизу.
3. Установите Arduino, L298N и LM2596 сверху на стойках.
4. Закрепите SG90 в передней части. Вал сервопривода должен быть вертикальным.
5. Установите HC-SR04 на качалку сервопривода так, чтобы два круглых излучателя смотрели горизонтально.
6. Поверните вал SG90 примерно в среднее положение и только затем прикрутите качалку. Датчик должен смотреть прямо при угле около 90°.
7. Убедитесь, что при повороте провода датчика не натягиваются и не попадают в колёса.

Схема соединений

Откуда	Куда
Arduino D5	L298N ENA
Arduino D2	L298N IN1
Arduino D3	L298N IN2
Arduino D4	L298N IN3
Arduino D7	L298N IN4
Arduino D6	L298N ENB
L298N OUT1 и OUT2	левый мотор
L298N OUT3 и OUT4	правый мотор
Arduino A0	HC-SR04 TRIG
Arduino A1	HC-SR04 ECHO
Arduino D8	оранжевый сигнальный провод SG90

У HC-SR04 выводы слева направо обычно подписаны VCC, TRIG, ECHO, GND. Проверяйте надписи именно на своей плате.

Проверка датчика до установки

Загрузите короткий тест, откройте монитор порта на 9600 бод и поднесите коробку на 10, 20 и 50 см.

const byte TRIG_PIN = A0;
const byte ECHO_PIN = A1;

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.begin(9600);
}

void loop() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH, 25000UL);
  float distance = duration == 0 ? 0 : duration * 0.0343F / 2.0F;
  Serial.println(distance);
  delay(200);
}

Показания должны приблизительно соответствовать линейке. Ноль означает, что эхо не пришло за 25 мс: проверьте провода и положение предмета.

Программа

Библиотека Servo входит в стандартную установку Arduino IDE. В этом проекте PWM используется на D5 и D6: библиотека Servo на Arduino Uno отключает PWM на D9 и D10, поэтому эти выводы здесь не выбраны.

#include <Servo.h>

const byte EN_LEFT = 5;
const byte EN_RIGHT = 6;
const byte IN1 = 2;
const byte IN2 = 3;
const byte IN3 = 4;
const byte IN4 = 7;
const byte SERVO_PIN = 8;
const byte TRIG_PIN = A0;
const byte ECHO_PIN = A1;

const int FORWARD_SPEED = 155;
const int BACK_SPEED = 135;
const int TURN_SPEED = 165;
const int STOP_DISTANCE_CM = 25;
const int CENTER_ANGLE = 90;
const int RIGHT_ANGLE = 30;
const int LEFT_ANGLE = 150;

Servo scanner;

void setMotor(byte pin1, byte pin2, byte enablePin, int power) {
  power = constrain(power, -255, 255);
  digitalWrite(pin1, power > 0 ? HIGH : LOW);
  digitalWrite(pin2, power < 0 ? HIGH : LOW);
  analogWrite(enablePin, abs(power));
}

void drive(int leftPower, int rightPower) {
  setMotor(IN1, IN2, EN_LEFT, leftPower);
  setMotor(IN3, IN4, EN_RIGHT, rightPower);
}

int oneMeasurementCm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH, 25000UL);
  if (duration == 0) return 300;
  return constrain((int)(duration * 0.0343F / 2.0F), 2, 300);
}

int distanceCm() {
  int a = oneMeasurementCm();
  delay(35);
  int b = oneMeasurementCm();
  delay(35);
  int c = oneMeasurementCm();

  if (a > b) { int t = a; a = b; b = t; }
  if (b > c) { int t = b; b = c; c = t; }
  if (a > b) { int t = a; a = b; b = t; }
  return b;
}

int lookAt(int angle) {
  scanner.write(angle);
  delay(450);
  return distanceCm();
}

void setup() {
  pinMode(EN_LEFT, OUTPUT);
  pinMode(EN_RIGHT, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  drive(0, 0);
  scanner.attach(SERVO_PIN);
  scanner.write(CENTER_ANGLE);
  delay(700);
}

void loop() {
  int front = distanceCm();

  if (front > STOP_DISTANCE_CM) {
    drive(FORWARD_SPEED, FORWARD_SPEED);
    delay(40);
    return;
  }

  drive(0, 0);
  delay(150);
  drive(-BACK_SPEED, -BACK_SPEED);
  delay(300);
  drive(0, 0);

  int right = lookAt(RIGHT_ANGLE);
  int left = lookAt(LEFT_ANGLE);
  scanner.write(CENTER_ANGLE);
  delay(300);

  if (right >= left) {
    drive(TURN_SPEED, -TURN_SPEED);
  } else {
    drive(-TURN_SPEED, TURN_SPEED);
  }

  delay(420);
  drive(0, 0);
  delay(100);
}

Первый запуск и настройка

1. Поднимите колёса. Включите питание и проверьте, что датчик смотрит прямо.
2. Если SG90 упирается в край, выключите питание и измените CENTER_ANGLE, RIGHT_ANGLE и LEFT_ANGLE.
3. Поставьте коробку в 15 см перед датчиком. Робот должен отъехать назад и начать обзор.
4. Проверьте направление обоих колёс. Если команда «вперёд» вращает одно назад, поменяйте местами два провода этого мотора.
5. На полу поставьте две коробки, оставив более широкий проход справа. Робот должен предпочесть правую сторону.
6. Если робот задевает препятствие, увеличьте STOP_DISTANCE_CM до 30–35.
7. Если робот не успевает развернуться, увеличьте задержку 420 мс; если перекручивается — уменьшите.

Диагностика

Симптом	Причина и исправление
Всегда показывается 300 см	перепутаны TRIG и ECHO, нет общей земли или предмет плохо отражает звук
SG90 дёргается, Arduino перезапускается	LM2596 не держит ток, слабые аккумуляторы или плохой контакт питания
Датчик смотрит вбок при 90°	переставьте качалку на валу или измените CENTER_ANGLE
Робот поворачивает к более близкой стене	перепутаны углы LEFT_ANGLE и RIGHT_ANGLE либо моторы
Робот видит пол	датчик наклонён вниз; выровняйте его горизонтально
Колёса не меняют скорость	не сняты перемычки ENA и ENB или D5/D6 подключены неверно

Практика

1. Измерьте реальное расстояние остановки при STOP_DISTANCE_CM, равном 15, 25 и 35.
2. Проверьте коробку, ножку стула и мягкую штору. Запишите, какие препятствия HC-SR04 видит хуже.
3. Постройте полосу из коробок шириной около 80 см и посчитайте успешные проезды из десяти.

Проверьте себя

• Почему датчик установлен на сервоприводе?
• Зачем перед обзором робот сначала останавливается и отъезжает?
• Почему нулевую длительность эха нельзя считать расстоянием 0 см?
• Как изменить безопасную дистанцию?
• Почему робот нельзя запускать возле лестницы?

Словарь статьи

• HC-SR04 — датчик, измеряющий время прохождения ультразвукового импульса до предмета и обратно.
• SG90 — малогабаритный сервопривод с управлением положением.
• Медиана — среднее по порядку из трёх значений; отбрасывает одиночный выброс.
• Порог — значение, после которого программа меняет действие.
• Конечный автомат — последовательность состояний: ехать, остановиться, осмотреться и повернуть.

Связанные темы

• Датчики расстояния
• Сервоприводы
• PWM-управление

Источники

• Arduino Project Hub: Obstacle avoiding robot — исходная опубликованная сборка и фотография.
• Arduino Servo library — официальная библиотека управления сервоприводами.
• SparkFun: HC-SR04 Ultrasonic Distance Sensor Hookup Guide — принцип работы и выводы датчика.
• STMicroelectronics: L298 — техническое описание драйвера.