Проект 6: Следование за рукой
Введение
Робот Фобо учится следовать за объектами! 👋
Помните, мы называли робота «Фобо» от слова «фобия» (страх)? В этом проекте проявится один из его «страхов» — страх потерять хозяина! Робот будет следовать за вашей рукой (или любым объектом), поддерживая безопасную дистанцию 20-30 см.
Что мы будем делать:
Научим робота измерять расстояние до объекта с помощью RCWL-9610A
Создадим алгоритм следования на безопасном расстоянии
Запрограммируем 3 режима: приближение, отступление, остановка
Добавим «мёртвую зону» для стабильной работы
Сделаем улучшенную версию со сканированием сервоприводом
Почему это важно:
Следование за объектом — это базовый алгоритм автономной робототехники. Он используется в роботах-тележках, дронах-операторах, роботах-помощниках. Научившись поддерживать дистанцию до объекта, мы заложим основу для более сложного поведения!
Примечание
Предварительные требования:
Проект 5: Работа с RCWL-9610A (ультразвуковой датчик)
Проект 4: Управление сервоприводом
Проект 2: Управление моторами
Проект 1: Сборка робота
Что нам понадобится
Компоненты
Всё уже установлено на роботе Фобо:
Компонент |
Примечание |
|---|---|
Робот Фобо в сборе |
После Проекта 1 |
RCWL-9610A ультразвуковой датчик |
Подключён к D3 (Trig), D7 (Echo) |
SG92R сервопривод |
Подключён к D9, датчик установлен на нём |
L298N + 4 мотора |
Управление движением |
2 аккумулятора 18650 |
Полностью заряжены |
Дополнительно
Просторное место — минимум 3 метра для движения
Ваша рука — или картонка/книга в качестве объекта
Программное обеспечение
Arduino IDE
Библиотека AlashUltrasonic (установлена в Проекте 5)
Библиотека Servo (встроенная)
Теория: Алгоритм следования за объектом
Принцип работы
Задача: Робот должен постоянно поддерживать расстояние 30-40 см до объекта перед собой.
Логика:
Измерить расстояние до объекта
Сравнить с целевым диапазоном (30-40 см)
Принять решение:
Если объект слишком близко (<30 см) → ехать назад
Если объект слишком далеко (>40 см) → ехать вперёд
Если объект в целевом диапазоне (30-40 см) → стоять
(Схема: три зоны - красная «слишком близко», зелёная «идеально», синяя «слишком далеко»)
Проблема: Дребезг (Jitter)
Что может пойти не так?
Если робот стоит на границе зоны (например, ровно на 40 см), датчик может показывать то 39.8 см, то 40.2 см из-за небольших измерительных погрешностей. Робот будет постоянно дёргаться: вперёд → стоп → вперёд → стоп.
Решение: Мёртвая зона (Dead Zone)
Добавим небольшой гистерезис — диапазон, внутри которого робот не меняет своё действие:
Расстояние < 15 см → БЫСТРО НАЗАД (опасно!)
15-25 см → МЕДЛЕННО НАЗАД
25-30 см → МЁРТВАЯ ЗОНА (продолжаем предыдущее действие)
30-40 см → СТОП (идеальная дистанция)
40-60 см → МЕДЛЕННО ВПЕРЁД
60-120 см → БЫСТРО ВПЕРЁД
> 400 см → ОШИБКА ДАТЧИКА (стоп)
Такой подход делает движение плавным и стабильным.
Расширенный алгоритм с 6 зонами
Для более естественного поведения используем 6 зон с разной скоростью:
Расстояние |
Действие |
Скорость |
|---|---|---|
< 15 см |
БЫСТРО НАЗАД |
50 (50%) |
15-25 см |
МЕДЛЕННО НАЗАД |
40 (40%) |
25-30 см |
МЁРТВАЯ ЗОНА (продолжаем) |
не меняется |
30-40 см |
СТОП |
0 |
40-60 см |
МЕДЛЕННО ВПЕРЁД |
40 (40%) |
> 60 см |
БЫСТРО ВПЕРЁД |
50 (50%) |
Это делает робота более «умным» — он не бросается вперёд-назад, а аккуратно подстраивается. Мёртвая зона устраняет дребезг на границе между «медленно назад» и «стоп».
Базовый код: Простое следование
Начнём с простого алгоритма с 3 зонами.
Код версия 1: Простая логика
/*
* Проект 6: Следование за рукой (версия 1 - базовая)
* Робот Фобо поддерживает дистанцию 20-30 см
*/
#include <AlashUltrasonic.h>
#include <AlashMotorControlLite.h>
// Пины датчика RCWL-9610A
const uint8_t TRIGGER_PIN = 3;
const uint8_t ECHO_PIN = 7; // Echo на D7
AlashUltrasonic sensor(TRIGGER_PIN, ECHO_PIN);
// Создаём два объекта моторов (L298N: режим DIR_DIR_PWM)
AlashMotorControlLite motorRight(DIR_DIR_PWM, 4, 2, 5); // IN1, IN2, ENA
AlashMotorControlLite motorLeft(DIR_DIR_PWM, 8, 12, 6); // IN3, IN4, ENB
// Параметры следования
const int MIN_DISTANCE = 30; // см - слишком близко
const int MAX_DISTANCE = 40; // см - слишком далеко
void setup() {
// Инициализация датчика
sensor.begin();
Serial.begin(9600);
Serial.println("========================================");
Serial.println(" Робот Фобо: Следование за рукой");
Serial.println("========================================");
Serial.print("Целевая дистанция: ");
Serial.print(MIN_DISTANCE);
Serial.print("-");
Serial.print(MAX_DISTANCE);
Serial.println(" см");
Serial.println("\nПоднесите руку к датчику...");
delay(2000);
}
void loop() {
// Измеряем расстояние
float distance = sensor.getDistance();
// Выводим в Serial Monitor
Serial.print("Расстояние: ");
Serial.print(distance, 1);
Serial.print(" см | ");
// Проверка на ошибку датчика (> 400 см)
if (distance > 400) {
Serial.println("❌ Ошибка датчика (слишком далеко)");
motorLeft.stop(); // Останавливаемся при ошибке
motorRight.stop();
delay(100);
return;
}
// Принимаем решение
if (distance < MIN_DISTANCE) {
// Слишком близко — едем назад
Serial.println("← НАЗАД (слишком близко)");
motorLeft.setSpeed(-40); // Отрицательная скорость = назад
motorRight.setSpeed(-40);
}
else if (distance > MAX_DISTANCE) {
// Слишком далеко — едем вперёд
Serial.println("→ ВПЕРЁД (догоняем)");
motorLeft.setSpeed(40); // Положительная скорость = вперёд
motorRight.setSpeed(40);
}
else {
// В целевом диапазоне — стоим
Serial.println("■ СТОП (идеальная дистанция)");
motorLeft.stop(); // Остановка моторов
motorRight.stop();
}
delay(100); // Обновление 10 раз в секунду
}
Как работает:
Каждые 100 мс измеряет расстояние
Проверка на ошибку датчика (> 400 см)
Если < 30 см → назад
Если > 40 см → вперёд
Если 30-40 см → стоп
Цикл повторяется
Улучшенный код: 5 зон + плавность
Добавим мёртвую зону и разную скорость.
Код версия 2: С мёртвой зоной
/*
* Проект 6: Следование за рукой (версия 2 - с мёртвой зоной)
* Плавное поведение без дребезга
*/
#include <AlashUltrasonic.h>
#include <AlashMotorControlLite.h>
const uint8_t TRIGGER_PIN = 3;
const uint8_t ECHO_PIN = 7; // Echo на D7
AlashUltrasonic sensor(TRIGGER_PIN, ECHO_PIN);
// Создаём два объекта моторов
AlashMotorControlLite motorRight(DIR_DIR_PWM, 4, 2, 5);
AlashMotorControlLite motorLeft(DIR_DIR_PWM, 8, 12, 6);
// Расширенные параметры с мёртвой зоной
const int ZONE_TOO_CLOSE = 15; // < 15 см - быстро назад
const int ZONE_CLOSE = 25; // 15-25 см - медленно назад
const int ZONE_PERFECT_MIN = 30; // 30-40 см - идеально, стоять
const int ZONE_PERFECT_MAX = 40;
const int ZONE_FAR = 60; // 40-60 см - медленно вперёд
// > 60 см - быстро вперёд
void setup() {
sensor.begin();
Serial.begin(9600);
Serial.println("========================================");
Serial.println(" Робот Фобо: Следование v2");
Serial.println(" (с мёртвой зоной для плавности)");
Serial.println("========================================");
delay(2000);
}
void loop() {
float distance = sensor.getDistance();
Serial.print("Дистанция: ");
Serial.print(distance, 1);
Serial.print(" см | ");
// Проверка на ошибку датчика (> 400 см)
if (distance > 400) {
Serial.println("❌ Ошибка датчика");
motorLeft.stop();
motorRight.stop();
delay(100);
return;
}
// Логика с 6 зонами (с мёртвой зоной 25-30 см)
if (distance < ZONE_TOO_CLOSE) {
// Зона 1: < 15 см - ОПАСНО БЛИЗКО!
Serial.println("⚠️ БЫСТРО НАЗАД!");
motorLeft.setSpeed(-50); // Быстро назад (отрицательная скорость)
motorRight.setSpeed(-50);
}
else if (distance < ZONE_CLOSE) {
// Зона 2: 15-25 см - медленно отступаем
Serial.println("← Медленно назад");
motorLeft.setSpeed(-40); // Медленно назад
motorRight.setSpeed(-40);
}
else if (distance < ZONE_PERFECT_MIN) {
// Зона 3 (МЁРТВАЯ): 25-30 см - продолжаем предыдущее действие
Serial.println("░ Мёртвая зона (продолжаем)");
// Не меняем скорость - продолжаем то, что делали
}
else if (distance <= ZONE_PERFECT_MAX) {
// Зона 4: 30-40 см - ИДЕАЛЬНО!
Serial.println("✓ СТОП (идеальная дистанция)");
motorLeft.stop(); // Остановка
motorRight.stop();
}
else if (distance < ZONE_FAR) {
// Зона 5: 40-60 см - медленно приближаемся
Serial.println("→ Медленно вперёд");
motorLeft.setSpeed(40); // Медленно вперёд
motorRight.setSpeed(40);
}
else if (distance <= 400) {
// Зона 6: 60-400 см - объект далеко, догоняем!
Serial.println("→→ БЫСТРО ВПЕРЁД!");
motorLeft.setSpeed(50); // Быстро вперёд
motorRight.setSpeed(50);
}
delay(100);
}
Улучшения:
6 зон вместо 3 (включая мёртвую зону 25-30 см)
Разная скорость для разных зон
Мёртвая зона 25-30 см предотвращает дребезг — робот продолжает предыдущее действие
Более плавное и естественное поведение без рывков
Продвинутый код: Со сканированием
Добавим сервопривод для сканирования пространства!
Код версия 3: С сервоприводом
/*
* Проект 6: Следование за рукой (версия 3 - со сканированием)
* Робот ищет объект, поворачивая "голову"
*/
#include <AlashUltrasonic.h>
#include <AlashMotorControlLite.h>
#include <Servo.h>
const uint8_t TRIGGER_PIN = 3;
const uint8_t ECHO_PIN = 7; // Echo на D7
AlashUltrasonic sensor(TRIGGER_PIN, ECHO_PIN);
// Создаём два объекта моторов
AlashMotorControlLite motorRight(DIR_DIR_PWM, 4, 2, 5);
AlashMotorControlLite motorLeft(DIR_DIR_PWM, 8, 12, 6);
Servo headServo;
const int SERVO_PIN = 9;
const int SERVO_CENTER = 90; // Калибровка центра серво (90 = стандарт, настройте под ваш серво)
const int ZONE_TOO_CLOSE = 15;
const int ZONE_CLOSE = 25;
const int ZONE_PERFECT_MIN = 30;
const int ZONE_PERFECT_MAX = 40;
const int ZONE_FAR = 60;
const int SCAN_TIMEOUT = 120; // см - если > 120 см, объекта нет
void setup() {
sensor.begin();
headServo.attach(SERVO_PIN);
headServo.write(SERVO_CENTER); // Смотрим вперёд
Serial.begin(9600);
Serial.println("========================================");
Serial.println(" Робот Фобо: Следование v3");
Serial.println(" (с поиском объекта)");
Serial.println("========================================");
delay(2000);
}
// Функция поиска объекта (сканирование)
void searchForObject() {
Serial.println("🔍 Ищу объект...");
motorLeft.stop(); // Останавливаемся для сканирования
motorRight.stop();
// Сканируем лево-центр-право
int angles[] = {SERVO_CENTER, 45, 135, 0, 180, SERVO_CENTER}; // Центр → Лево → Право → Дальше лево → Дальше право → Центр
for (int i = 0; i < 6; i++) {
headServo.write(angles[i]);
delay(300); // Ждём стабилизации серво
float dist = sensor.getDistance();
Serial.print(" Угол ");
Serial.print(angles[i]);
Serial.print("°: ");
Serial.print(dist, 1);
Serial.println(" см");
if (dist < SCAN_TIMEOUT) {
// Объект найден!
Serial.println(" ✓ Объект обнаружен!");
// Поворачиваем робота к объекту
if (angles[i] < 70) {
Serial.println(" Поворачиваю направо...");
motorLeft.setSpeed(60); // Левый вперёд (больше силы для поворота)
motorRight.setSpeed(-60); // Правый назад = поворот направо
delay(500);
}
else if (angles[i] > 110) {
Serial.println(" Поворачиваю налево...");
motorLeft.setSpeed(-60); // Левый назад (больше силы для поворота)
motorRight.setSpeed(60); // Правый вперёд = поворот налево
delay(500);
}
motorLeft.stop();
motorRight.stop();
headServo.write(SERVO_CENTER); // Возвращаем голову вперёд
delay(300);
return; // Выходим из поиска
}
}
// Объект не найден нигде
Serial.println(" ✗ Объект не найден. Остаюсь на месте.");
headServo.write(SERVO_CENTER);
delay(300);
}
void loop() {
// Смотрим вперёд
headServo.write(SERVO_CENTER);
delay(50);
float distance = sensor.getDistance();
Serial.print("Дистанция: ");
Serial.print(distance, 1);
Serial.print(" см | ");
// Проверка на ошибку датчика (> 400 см)
if (distance > 400) {
Serial.println("❌ Ошибка датчика");
motorLeft.stop();
motorRight.stop();
delay(100);
return;
}
// Если объекта нет (> 120 см), запускаем поиск
if (distance > SCAN_TIMEOUT) {
Serial.println("❌ Объект потерян!");
searchForObject();
delay(1000);
return;
}
// Обычная логика следования (с мёртвой зоной)
if (distance < ZONE_TOO_CLOSE) {
// Зона 1: < 15 см - ОПАСНО БЛИЗКО!
Serial.println("⚠️ БЫСТРО НАЗАД!");
motorLeft.setSpeed(-50); // Быстро назад
motorRight.setSpeed(-50);
}
else if (distance < ZONE_CLOSE) {
// Зона 2: 15-25 см - медленно отступаем
Serial.println("← Медленно назад");
motorLeft.setSpeed(-40); // Медленно назад
motorRight.setSpeed(-40);
}
else if (distance < ZONE_PERFECT_MIN) {
// Зона 3 (МЁРТВАЯ): 25-30 см - продолжаем предыдущее действие
Serial.println("░ Мёртвая зона (продолжаем)");
// Не меняем скорость - продолжаем то, что делали
}
else if (distance <= ZONE_PERFECT_MAX) {
// Зона 4: 30-40 см - ИДЕАЛЬНО!
Serial.println("✓ СТОП (идеально)");
motorLeft.stop(); // Остановка
motorRight.stop();
}
else if (distance < ZONE_FAR) {
// Зона 5: 40-60 см - медленно приближаемся
Serial.println("→ Медленно вперёд");
motorLeft.setSpeed(40); // Медленно вперёд
motorRight.setSpeed(40);
}
else if (distance <= SCAN_TIMEOUT) {
// Зона 6: 60-120 см - объект далеко, догоняем!
Serial.println("→→ БЫСТРО ВПЕРЁД!");
motorLeft.setSpeed(50); // Быстро вперёд
motorRight.setSpeed(50);
}
delay(100);
}
Новые возможности:
Функция searchForObject() — сканирует пространство на 180°
Если объект потерян (>120 см), робот ищет его, поворачивая «голову»
Когда объект найден, робот поворачивается к нему корпусом
Повышенная мощность поворотов (60 вместо 40-50) — для надёжного разворота на месте
Более «умное» поведение — робот не теряет объект
Загрузка и тестирование
Шаг 1: Выбор версии кода
Выберите версию в зависимости от опыта:
Версия 1 (базовая) — для начинающих, простая логика
Версия 2 (с зонами) — рекомендуется, плавное поведение
Версия 3 (со сканированием) — продвинутая, с поиском объекта
Шаг 2: Подготовка кода
Настройте зоны под свои предпочтения (опционально)
Настройте скорости для разных зон (опционально)
Проверьте код на ошибки (✓)
Шаг 3: Загрузка
Опасно
ВАЖНО: Выньте батареи перед загрузкой! Питание от USB.
Выньте батареи
Подключите USB
Загрузите программу (→)
Откройте Serial Monitor (9600 baud)
Шаг 4: Тестирование
Отключите USB
Вставьте батареи
Поставьте робота на пол
Поднесите руку на расстояние ~35 см перед датчиком
Медленно отводите руку — робот должен ехать за ней
Приближайте руку — робот должен отступать
Что должно происходить:
Рука далеко (>40 см) → робот едет вперёд
Рука близко (<30 см) → робот едет назад
Рука на 30-40 см → робот стоит на месте
Эксперименты
Эксперимент 1: Настройка зон
Цель: Подобрать оптимальные зоны для вашего робота.
Задание:
Измените ZONE_PERFECT_MIN и ZONE_PERFECT_MAX на 25-35 см
Протестируйте — стало лучше или хуже?
Попробуйте разные комбинации: ближе (20-30 см) или дальше (40-50 см)
Найдите идеальные значения для вашего робота
Эксперимент 2: Режим «следование за стеной»
Цель: Робот едет вдоль стены на постоянной дистанции.
Подсказка:
Установите датчик под углом 90° (смотрит вбок), используйте код версии 2, но вместо вперёд/назад используйте повороты:
if (distance < 20) {
// Слишком близко к стене — поворачиваем от стены
motorLeft.setSpeed(40); // Левый вперёд
motorRight.setSpeed(-40); // Правый назад = поворот направо (от стены)
}
else if (distance > 30) {
// Далеко от стены — поворачиваем к стене
motorLeft.setSpeed(-40); // Левый назад
motorRight.setSpeed(40); // Правый вперёд = поворот налево (к стене)
}
else {
// Идеально — едем прямо
motorLeft.setSpeed(40);
motorRight.setSpeed(40);
}
Эксперимент 3: Режим «убегание»
Цель: Робот убегает от руки (противоположное поведение).
Задание:
Инвертируйте логику:
if (distance < 30) {
// Рука близко — убегаем!
motorLeft.setSpeed(50); // Быстро вперёд
motorRight.setSpeed(50);
}
else {
// Рука далеко — можно расслабиться
motorLeft.stop();
motorRight.stop();
}
Эксперимент 4: Режим «любопытный робот»
Цель: Робот подъезжает очень близко (5-10 см), «рассматривает» объект.
Задание:
Измените зоны на очень маленькие:
const int ZONE_PERFECT_MIN = 5;
const int ZONE_PERFECT_MAX = 10;
Поиск неисправностей
Проблема: Робот стоит на месте, не реагирует на руку
Причина: Датчик не работает или неправильно подключён
Решение:
Откройте Serial Monitor — видите ли расстояния?
Если расстояние всегда 0 или 400 см → проверьте подключение датчика
Проверьте: Trig=D3, Echo=D7
Вызывается ли sensor.begin() в setup()?
Проблема: Датчик иногда показывает большие значения (400-800 см)
Причина: Ультразвуковой датчик не получил эхо (объект слишком далеко, или поглощает звук)
Решение:
Это нормально! Датчик возвращает большое значение, когда не видит объектов
В коде добавлена проверка: if (distance > 400) — робот останавливается при ошибке
Если это происходит часто — проверьте, нет ли рядом мягких поверхностей (ковёр, шторы)
Проблема: Робот постоянно дёргается вперёд-назад
Причина: Дребезг на границе зон
Решение:
Используйте версию 2 с мёртвой зоной
Увеличьте размер идеальной зоны (например, 20-35 см)
Добавьте задержку delay(200) между измерениями
Проблема: Робот едет криво при следовании
Причина: Один мотор быстрее другого
Решение:
Найдите все места в коде, где устанавливается одинаковая скорость
Попробуйте снизить скорость одного из моторов:
// Было: motorLeft.setSpeed(40); motorRight.setSpeed(40); // Стало (если робот уходит вправо): motorLeft.setSpeed(40); motorRight.setSpeed(36); // Правый на 10% медленнее (40 * 0.9)
Подберите коэффициент (0.85-0.95) для прямолинейного движения
Проблема: Сервопривод дёргается или не поворачивается
Причина: Недостаточно питания или неправильное подключение
Решение:
Проверьте заряд батарей (>7.5V)
Проверьте подключение серво: GND, 5V, Signal=D9
Добавьте delay(300) после servo.write() для стабилизации
Проблема: Робот не может развернуться на месте (версия 3)
Причина: Недостаточная мощность для поворота — трение колёс о поверхность
Решение:
Увеличьте скорость поворота с 40 до 60 в функции searchForObject():
// Поворот направо motorLeft.setSpeed(60); // Было 40 motorRight.setSpeed(-60); // Было -40 // Поворот налево motorLeft.setSpeed(-60); // Было -40 motorRight.setSpeed(60); // Было 40
Если 60 недостаточно, попробуйте 70-80 (максимум 100)
Проверьте заряд батарей — низкий заряд уменьшает мощность моторов
Тестируйте на гладкой поверхности (линолеум, ламинат) — на ковре нужна бОльшая мощность
Проблема: Робот теряет объект и не ищет его
Причина: Используется версия 1 или 2 без поиска
Решение:
Используйте версию 3 со сканированием
Или добавьте свою логику поиска
Советы и улучшения
Совет 1: Фильтрация шумов датчика
Датчик может давать случайные выбросы. Усредните 3 измерения:
float getFilteredDistance() {
float sum = 0;
for (int i = 0; i < 3; i++) {
sum += sensor.getDistance();
delay(30);
}
return sum / 3.0;
}
Совет 2: Динамическая скорость
Скорость зависит от расстояния — чем дальше объект, тем быстрее едем:
int speed = map(distance, 30, 100, 40, 60); // От 40% до 60%
speed = constrain(speed, 40, 60);
motorLeft.setSpeed(speed); // Передаём скорость напрямую
motorRight.setSpeed(speed);
Совет 3: Звуковая индикация зон
Добавьте buzzer для звукового сигнала в разных зонах:
if (distance < ZONE_TOO_CLOSE) {
tone(13, 2000, 50); // Высокий звук - опасность!
}
Заключение
Поздравляем! 🎉 Вы создали интерактивного робота, который следует за объектами!
Что вы узнали:
✅ Алгоритм следования за объектом на постоянной дистанции
✅ Проблема дребезга и решение через мёртвую зону (25-30 см)
✅ Использование 6 зон для плавного поведения без рывков
✅ Интеграция RCWL-9610A и моторов в единую систему
✅ Сканирование пространства сервоприводом для поиска объекта
✅ Фильтрация шумов датчика
Что дальше:
Вы освоили взаимодействие робота с объектами! В следующих проектах изучим другие автономные режимы:
Проект 7: Объезд препятствий — робот сканирует и объезжает препятствия автоматически
Проект 8: Датчики линии — знакомство с 3 аналоговыми IR-датчиками
Проект 9: Следование по линии — робот едет по чёрной линии, используя 3 датчика линии
Проект 14: Мастер-режим — все режимы в одной программе с переключением
Практическое применение:
Алгоритм следования используется в:
Роботах-тележках в магазинах
Дронах-операторах для съёмки
Роботах-помощниках для пожилых людей
Автономных транспортных средствах
Удачи в создании умных роботов! 🤖👋
Примечание
Идея для проекта: Попробуйте объединить следование за рукой с объездом препятствий — робот следует за рукой, но объезжает стулья и стены на пути!