Проект 7: Объезд препятствий

Робот Фобо становится по-настоящему автономным — он самостоятельно объезжает препятствия! Датчик на сервоприводе поворачивается влево-вправо, сканирует окружение, и робот выбирает свободное направление. Один из самых зрелищных режимов работы!

Как это работает:

Робот едет вперёд, постоянно измеряя расстояние впереди
При обнаружении препятствия (<20 см) — остановка
Датчик поворачивается на 5 углов (30°, 60°, 90°, 120°, 150°) и измеряет расстояния
Робот поворачивает туда, где больше свободного пространства
Продолжает движение вперёд

Рабочий код

Скопируйте и загрузите этот код в Arduino IDE:

/*
 * Проект 7: Объезд препятствий
 * Робот Фобо объезжает препятствия с 5-точечным сканированием
 */

#include <Servo.h>
#include <AlashUltrasonic.h>
#include <AlashMotorControlLite.h>

// ============ ПИНЫ ============
const uint8_t trigPin = 3;
const uint8_t echoPin = 7;     // Echo на D7
const uint8_t servoPin = 9;
const int SERVO_CENTER = 100;  // Калибровка центра серво (90 = стандарт, настройте под ваш серво)

// ============ НАСТРОЙКИ ============
const int safeDistance = 35;       // Безопасное расстояние (см)
const int criticalDistance = 25;   // Критическое расстояние (см)
const int speedFast = 75;          // Скорость быстро (-100...100)
const int speedNormal = 60;        // Скорость нормально
const int speedSlow = 40;          // Скорость медленно
const int speedTurn = 65;          // Скорость поворота

// Углы сканирования (5 точек)
const int scanAngles[5] = {30, 60, SERVO_CENTER, 120, 150};
float scanDistances[5];

// Время поворота на 90° (мс) — можно калибровать
const int turnTime90 = 550;

// ============ ОБЪЕКТЫ ============
AlashUltrasonic sensor(trigPin, echoPin);
Servo headServo;
AlashMotorControlLite motorRight(DIR_DIR_PWM, 4, 2, 5);   // IN1, IN2, ENA
AlashMotorControlLite motorLeft(DIR_DIR_PWM, 8, 12, 6); // IN3, IN4, ENB

void setup() {
  Serial.begin(9600);
  sensor.begin();

  headServo.attach(servoPin);
  headServo.write(SERVO_CENTER);  // Датчик смотрит вперёд
  delay(500);

  Serial.println("=== Робот Фобо: Объезд препятствий ===");
  Serial.println("Старт через 3 секунды...");
  delay(3000);
}

void loop() {
  // Измеряем расстояние впереди
  float distanceAhead = sensor.getDistance();

  Serial.print("Впереди: ");
  Serial.print(distanceAhead, 1);
  Serial.print(" см | ");

  // Адаптивная скорость в зависимости от расстояния
  if (distanceAhead > safeDistance * 2) {
    moveForward(speedFast);
    Serial.println("→→ Быстро");
  }
  else if (distanceAhead > safeDistance) {
    moveForward(speedNormal);
    Serial.println("→ Нормально");
  }
  else if (distanceAhead > criticalDistance) {
    moveForward(speedSlow);
    Serial.println("→ Медленно");
  }
  else {
    // Препятствие слишком близко!
    Serial.println("⚠️  ПРЕПЯТСТВИЕ!");
    stopMotors();
    delay(300);

    // Сканирование 5 точек
    performScan();

    // Принятие решения
    makeDecision();

    delay(300);
  }

  delay(80);
}

// ============ ФУНКЦИЯ СКАНИРОВАНИЯ ============
void performScan() {
  Serial.println("🔍 Сканирование:");

  for (int i = 0; i < 5; i++) {
    headServo.write(scanAngles[i]);
    delay(300);  // Ждём стабилизации серво

    scanDistances[i] = sensor.getDistance();

    Serial.print("  ");
    Serial.print(scanAngles[i]);
    Serial.print("°: ");
    Serial.print(scanDistances[i], 1);
    Serial.println(" см");
  }

  // Возвращаем датчик в центр
  headServo.write(SERVO_CENTER);
  delay(200);
}

// ============ ПРИНЯТИЕ РЕШЕНИЯ ============
void makeDecision() {
  // Находим направление с максимальным расстоянием
  int maxIndex = 0;
  float maxDistance = scanDistances[0];

  for (int i = 1; i < 5; i++) {
    if (scanDistances[i] > maxDistance) {
      maxDistance = scanDistances[i];
      maxIndex = i;
    }
  }

  Serial.print("💡 Лучшее направление: ");
  Serial.print(scanAngles[maxIndex]);
  Serial.print("° (");
  Serial.print(maxDistance, 1);
  Serial.println(" см)");

  // Проверка: все стороны заблокированы?
  bool allBlocked = true;
  for (int i = 0; i < 5; i++) {
    if (scanDistances[i] > criticalDistance) {
      allBlocked = false;
      break;
    }
  }

  if (allBlocked) {
    Serial.println("🔄 Все стороны заблокированы! Отъезд назад");
    moveBackward();
    delay(800);
    stopMotors();
    turnAround();
    return;
  }

  // Поворот в лучшем направлении
  if (maxIndex == 0) {
    // 30° - крайний правый
    Serial.println("↗↗ Резкий поворот НАПРАВО");
    turnRight90();
    delay(300);
    turnRight90();  // Двойной поворот
  }
  else if (maxIndex == 1) {
    // 60° - правый
    Serial.println("↗ Поворот НАПРАВО");
    turnRight90();
  }
  else if (maxIndex == 2) {
    // 90° - центр (прямо свободно)
    Serial.println("→ Продолжаем ПРЯМО");
    // Не поворачиваем
  }
  else if (maxIndex == 3) {
    // 120° - левый
    Serial.println("↖ Поворот НАЛЕВО");
    turnLeft90();
  }
  else {
    // 150° - крайний левый
    Serial.println("↖↖ Резкий поворот НАЛЕВО");
    turnLeft90();
    delay(300);
    turnLeft90();  // Двойной поворот
  }
}

// ============ ФУНКЦИИ ДВИЖЕНИЯ ============
void moveForward(int speed) {
  motorLeft.setSpeed(speed);
  motorRight.setSpeed(speed);
}

void moveBackward() {
  motorLeft.setSpeed(-speedNormal);
  motorRight.setSpeed(-speedNormal);
}

void turnLeft90() {
  motorLeft.setSpeed(-speedTurn);   // Левый назад
  motorRight.setSpeed(speedTurn);   // Правый вперёд
  delay(turnTime90);
  stopMotors();
}

void turnRight90() {
  motorLeft.setSpeed(speedTurn);    // Левый вперёд
  motorRight.setSpeed(-speedTurn);  // Правый назад
  delay(turnTime90);
  stopMotors();
}

void turnAround() {
  // Разворот на 180°
  motorLeft.setSpeed(-speedTurn);   // Левый назад
  motorRight.setSpeed(speedTurn);   // Правый вперёд
  delay(turnTime90 * 2);
  stopMotors();
}

void stopMotors() {
  motorLeft.stop();
  motorRight.stop();
}

Загрузка и запуск

Библиотеки: AlashUltrasonic, AlashMotorControlLite, Servo (установка см. Урок 1 и Проект 5)

Загрузка: Без батарей, через USB (инструкции см. Урок 1)

Тестирование:

Отключите USB, вставьте батареи
Создайте полосу препятствий (коробки, книги высотой >10 см)
Включите питание — робот стартует через 3 секунды

Совет

Требования к препятствиям:

Высота не менее 10 см (чтобы датчик «видел»)
Матовые поверхности (картон, дерево, пластик)
Не прозрачные и не зеркальные

Калибровка времени поворота

Если робот поворачивает не на 90°, откалибруйте параметр turnTime90:

Тест: Поставьте робота на пол, отметьте начальное положение (приклейте листок бумаги), запустите. Робот повернёт при первом препятствии. Измерьте угол поворота:

Меньше 90° → увеличьте turnTime90 (например, с 550 до 600)
Больше 90° → уменьшите turnTime90 (например, с 550 до 500)

Формула пересчёта:

Новое_время = (Текущее_время × 90°) / Измеренный_угол

Пример: при turnTime90 = 550 робот повернул на 75°:

Новое_время = (550 × 90) / 75 = 660 мс

Эксперименты

Эксперимент 1: Безопасное расстояние

Попробуйте разные значения safeDistance:

const int safeDistance = 25;  // Агрессивная езда (близко к препятствиям)
const int safeDistance = 35;  // Средняя (рекомендуется) ← по умолчанию
const int safeDistance = 50;  // Осторожная езда (большой запас)

Вопрос: Как меняется поведение робота?

Эксперимент 2: Количество точек сканирования

Сравните варианты:

3 точки (быстрое сканирование):

const int scanAngles[3] = {30, 90, 150};

5 точек (текущий вариант, сбалансированное):

const int scanAngles[5] = {30, 60, 90, 120, 150};

7 точек (детальное, но медленное):

const int scanAngles[7] = {15, 45, 75, 90, 105, 135, 165};

Не забудьте изменить размер массива и цикл for (int i = 0; i < N; i++)!

Эксперимент 3: Режим «всегда направо»

Измените функцию makeDecision() на простое правило:

void makeDecision() {
  // Если справа свободно — поворот направо
  if (scanDistances[0] > criticalDistance) {
    Serial.println("↗ НАПРАВО (по правилу правой руки)");
    turnRight90();
  }
  // Иначе если слева свободно — налево
  else if (scanDistances[4] > criticalDistance) {
    Serial.println("↖ НАЛЕВО");
    turnLeft90();
  }
  // Иначе разворот
  else {
    Serial.println("🔄 РАЗВОРОТ");
    turnAround();
  }
}

Задание: Какой режим быстрее проходит лабиринт — «умный» или «всегда направо»?

Решение проблем

Проблема 1: Робот врезается в препятствия

Причины:

Датчик направлен вниз/вверх → Убедитесь, что датчик смотрит горизонтально
Слишком маленькое safeDistance → Увеличьте до 30-40 см
Препятствие слишком низкое → Используйте препятствия выше 10 см

Проблема 2: Поворот не на 90°

Решение:

Откалибруйте turnTime90 (см. раздел «Калибровка»)
Проверьте заряд батарей (при низком заряде повороты неточные)

Проблема 3: Серво дёргается или шумит

Решение:

Увеличьте задержки после headServo.write() до 400-500 мс
Проверьте крепление датчика (не зажат ли сервопривод?)
Убедитесь, что питание стабильное (батареи заряжены >7.5V)

Проблема 4: Робот застревает в углах

Решение:

Уже реализовано: робот отъезжает назад и разворачивается при полной блокировке
Если недостаточно, увеличьте время отъезда: delay(1200); вместо delay(800);

Проблема 5: Датчик показывает случайные значения

Причины:

Блестящие/прозрачные препятствия → Используйте матовые поверхности
Острые углы препятствий → Ультразвук отражается в сторону

Решение: Добавьте фильтрацию (измерение 3 раза, среднее значение):

float getFilteredDistance() {
  float sum = 0;
  for (int i = 0; i < 3; i++) {
    sum += sensor.getDistance();
    delay(50);
  }
  return sum / 3.0;
}

// В loop() используйте:
float distanceAhead = getFilteredDistance();

Заключение

Поздравляем! 🎉 Вы создали автономного робота, который объезжает препятствия!

Что вы узнали:

✅ Алгоритм объезда препятствий (сканирование → решение → поворот)

✅ Управление сервоприводом для сканирования окружения

✅ Принятие решений на основе множественных измерений

✅ Калибровка поворотов для точного маневрирования

✅ Отладка поведения робота

Что дальше:

Проект 8: Датчики линии — знакомство с 3 аналоговыми IR-датчиками
Проект 9: Следование по линии — робот едет по чёрной линии
Проект 14: Мастер-режим — все режимы в одной программе!

Успехов в робототехнике! 🤖