Построение робота-следопыта с PID-регулятором и Android-интеграцией

Компоненты и материалы

Arduino Nano R3	1
Датчик слежения за линией (SparkFun RedBot или TCRT5000 4-канальный)	1 (или 5 отдельных)
Bluetooth-модуль HC-06	1
Android-устройство	1
Короб для 4х АА батареек (2 комплекта)	2
Сервоприводы непрерывного вращения (SM-S4303R)	2

Приложения и онлайн-сервисы

• Arduino IDE
• MIT App Inventor 2

О проекте

Цель проекта — построить робота-следопыта с PID-управлением. Также используем Android-устройство для легкой настройки основных параметров управления для быстрой и удобной настройки.

Это первая часть из двух более сложного проекта, исследующего потенциал роботов-следопытов. Во второй части: робот-лабиринт, использующий искусственный интеллект на Arduino — робот будет исследовать и решать лабиринты с простыми ИИ-техниками.

Шаг 1: Спецификация материалов

Список материалов прост, итоговый робот очень дешевый (около $75):

• Корпус (адаптируйте под себя): 2 деревянные квадраты 80x80мм, 3 биндер-клипса, 2 деревянных колеса (диаметр 50мм), 1 шарикоподшипник, 9 резинок, 3M Command полоска, пластиковые крепления для датчиков, макетная плата и провода.
• 2 комплекта 4х Ni-MH батареек (5В каждый)
• 2 сервопривода непрерывного вращения SM-S4303R
• Arduino Nano
• Bluetooth-модуль HC-06
• 5 датчиков линии (TCRT5000 4-канальный модуль + 1 отдельный)
• 1 LED, 1 кнопка

Шаг 2: Настройка моторов

Используются 2 сервопривода непрерывного вращения SM-S4303R. Они склеиваются в единый блок (3M Command, клей или двусторонний скотч). Сервоприводы управляются шириной импульса на входе данных: 1.0мс (1000мкс) — полная скорость в одну сторону, 1.5мс (1500мкс) — стоп, 2.0мс (2000мкс) — полная скорость в другую сторону. Промежуточные значения дают пропорциональную скорость.

После физического соединения сервоприводов подключите их по схеме (внешнее питание 5В или 6В) и сигнальные провода к Arduino:

• Левый сервопривод: пин 5 Arduino
• Правый сервопривод: пин 3 Arduino

Первое действие — подать импульс 1.5мс и проверить, что моторы стоят. Если нет — отрегулируйте желтым болтиком под сервоприводом. Если нет регулировки — меняйте параметр 1500мкс в коде до полной остановки.

Тестовый код для проверки:

#include <Servo.h>
Servo leftServo; Servo rightServo;
void setup() {
  leftServo.attach(5); rightServo.attach(3);
  leftServo.writeMicroseconds(1500);
  rightServo.writeMicroseconds(1500);
}
void loop() { }

Шаг 3: Сборка корпуса и тест движения

Собираем платформу: сервоприводы на нижнюю деревянную пластину (3M Command), вторую пластину сверху на биндер-клипсах, шарикоподшипник на клипсе. Батарея для моторов между макетной платой и рамой. Arduino Nano на макетной плате. GND источника питания моторов к GND Arduino. Сервоприводы к пинам 5 и 3. LED на пин 13. Кнопка на пин 9.

Из-за противоположного монтажа сервоприводов диапазоны скоростей:

• Правый вперед: 1500мкс (стоп) → 2000мкс (макс)
• Левый вперед: 1500мкс (стоп) → 1000мкс (макс)

Кнопка на пине 9 удобна для тестов и старта робота:

while(digitalRead(buttonPin)) { }
motorTurn(LEFT, 500); motorTurn(RIGHT, 500);

Программа ждет нажатия кнопки, затем выполняет повороты.

Шаг 4: Bluetooth-модуль HC-06 (опционально)

HC-06 на макетной плате. Библиотека SoftwareSerial. Подключение:

• Tx HC-06 → пин 10 Arduino (Rx)
• Rx HC-06 → пин 11 Arduino (Tx)
• VCC/GND → 5V/GND Arduino

Робот работает с Bluetooth и без него. Код написан так: если BT не активирован, используются параметры по умолчанию. Bluetooth и приложение опциональны — для соревнований и тонкой настройки.

Шаг 5: Установка датчиков линии

Подключение датчиков к пинам Arduino:

• Sensor 0 → пин 12
• Sensor 1 → пин 18 (A4)
• Sensor 2 → пин 17 (A3)
• Sensor 3 → пин 16 (A2)
• Sensor 4 → пин 19 (A5)

5 датчиков крепятся на пластиковую планку. Рекомендую подписать их 0 (левый) → 4 (правый). Провода под рамой, закреплены резинками, не мешают колесам.

Второй комплект батарей (5В) к Vin Arduino.

Шаг 6: Логика ИК-датчиков

ИК-датчик: ИК-светодиод и фототранзистор. Отраженный свет генерирует напряжение, пропорциональное отражающей способности поверхности (высокое для светлых, низкое для темных). Модуль выдает цифровой сигнал: HIGH (1) — темная/черная поверхность, LOW (0) — светлая. Потенциометр на модуле настраивает порог.

Массив из 5 датчиков смонтирован так, что при центрировании линии под средним датчиком (Sensor 2) срабатывает только он. Расстояние между датчиками выбрано так, чтобы два соседних датчика могли одновременно покрыть ширину черной линии.

Возможные выходы массива при движении по линии:

0 0 0 0 1
0 0 0 1 1
0 0 0 1 0
0 0 1 1 0
0 0 1 0 0  ← центр (оптимум)
0 1 1 0 0
0 1 0 0 0
1 1 0 0 0
1 0 0 0 0

Вводим переменную «error» (ошибка) для управления положением робота над линией. Оптимум: линия под Sensor 2 → вывод 0 0 1 0 0 → error = 0. Робот уходит влево (линия «уходит» вправо) → error положительный. Робот уходит вправо → error отрицательный.

Таблица соответствия:

0 0 1 0 0 → Error = 0
0 0 0 0 1 → Error = 4
0 0 0 1 1 → Error = 3
0 0 0 1 0 → Error = 2
0 0 1 1 0 → Error = 1
0 1 1 0 0 → Error = -1
0 1 0 0 0 → Error = -2
1 1 0 0 0 → Error = -3
1 0 0 0 0 → Error = -4

В коде датчикам присваиваются имена, значения считываются в массив LFSensor[5], затем логика if-else вычисляет error.

Шаг 7: Пропорциональное управление (P)

Базовая скорость: iniMotorPower = 250. Левый серво: 1500 - 250 = 1250мкс, правый: 1500 + 250 = 1750мкс — половина скорости.

При ошибке error = 1 (робот ушел влево): нужно повернуть вправо → уменьшить скорость правого (уменьшить импульс), увеличить левого (уменьшить импульс).

rightServo.writeMicroseconds(1500 + iniMotorPower - Kp*error);
leftServo.writeMicroseconds(1500 - iniMotorPower - Kp*error);

Коэффициент Kp (пропорциональный) усиливает реакцию. При Kp=50: error=1 → коррекция 50мкс. Это и есть «П» составляющая PID.

Шаг 8: PID-управление (опционально)

PID (Пропорционально-Интегрально-Дифференцирующее) — распространенный алгоритм управления. Представьте пружину: при смещении она стремится вернуться в исходное состояние максимально быстро.

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

• Error (e) = set_point - current_value (у нас error от позиции над линией)
• P = error — величина необходимого изменения
• I = I + error — сумма прошлых ошибок, устраняет статическую ошибку
• D = error - previousError — разница с предыдущей ошибкой, снижает перерегулирование

Уравнение: PIDvalue = (Kp×P) + (Ki×I) + (Kd×D)

Kp — величина изменения, Ki — скорость реакции, Kd — стабильность.

Настройка методом проб и ошибок:

1. Kd=0, настраиваем Kp. Kp=25 — хорошее начало (в шаге 7 использовали 50).
2. Когда робот реагирует адекватно, добавляем Kd. Начальные Kp=Kd=половина рабочего Kp (например, 25 и 25). Увеличивайте Kd для уменьшения перерегулирования, уменьшайте при нестабильности.
3. Также влияет частота цикла (delay в коде). Подбирайте экспериментально.

Функция расчета PID:

void calculatePID() {
  P = error;
  I = I + error;
  D = error - previousError;
  PIDvalue = (Kp*P) + (Ki*I) + (Kd*D);
  previousError = error;
}

Управление моторами:

void motorPIDcontrol() {
  int leftMotorSpeed = 1500 - iniMotorPower - PIDvalue;
  int rightMotorSpeed = 1500 + iniMotorPower - PIDvalue;
  leftServo.writeMicroseconds(leftMotorSpeed);
  rightServo.writeMicroseconds(rightMotorSpeed);
}

Шаг 9: Финальный код

Основной цикл:

void loop() {
  readLFSsensors(); // чтение датчиков, расчет error
  calculatePID();
  motorPIDcontrol();
}

Для полноты добавляем состояния (переменная mode):

• STOPPED = 0 — все датчики видят черную линию (1 1 1 1 1) → полная остановка
• FOLLOWING_LINE = 1 — нормальное следование
• NO_LINE = 2 — линия потеряна (0 0 0 0 0) → поворот 180° или поиск

Полный цикл с машиной состояний:

void loop() {
  readLFSsensors();
  switch (mode) {
    case STOPPED: motorStop(); break;
    case NO_LINE: motorStop(); motorTurn(LEFT, 180); break;
    case FOLLOWING_LINE: calculatePID(); motorPIDcontrol(); break;
  }
}

Полный код с инициализацией и дополнительной логикой доступен в репозитории GitHub (ссылка в конце).

Шаг 10: Настройка PID через Android-приложение

Лучший способ подобрать константы PID — метод проб и ошибок. Но перекомпиляция каждый раз замедляет процесс. Android-приложение позволяет отправлять константы на этапе настройки (Set-Up Phase).

Приложение создано в MIT App Inventor. Есть ручное управление: FW, BW, Left, Right, Stop (отправляют 'f','b','l','r','s' по BT). Три ползунка для PID:

• Kd: «d/XXX» (0-100)
• Kp: «p/XXX»
• Ki: «i/XXX»

Кнопка «Start» работает как кнопка на пине 9 Arduino.

Файлы .aia (для редактирования) и .apk (для установки) в разделе Code.

Шаг 11: Изменение кода для удаленной настройки PID

В setup() добавляем цикл ожидания настройки через BT:

while (digitalRead(buttonPin) && !mode) {
  checkBTcmd();      // проверка команд от BT
  manualCmd();       // выполнение ручных команд
  command = "";
}
checkPIDvalues();
mode = STOPPED;

Функция ручных команд:

void manualCmd() {
  switch (command[0]) {
    case 'g': mode = FOLLOWING_LINE; break;
    case 's': motorStop(); break;
    case 'f': motorForward(); break;
    case 'r': motorTurn(RIGHT, 30); motorStop(); break;
    case 'l': motorTurn(LEFT, 30); motorStop(); break;
    case 'b': motorBackward(); break;
    case 'p': Kp = command[2]; break;
    case 'i': Ki = command[2]; break;
    case 'd': Kd = command[2]; break;
  }
}

Шаг 12: Заключение

Это первая часть проекта, исследующего потенциал роботов-следопытов. Следующая часть — робот, решающий лабиринты на базе этого проекта. Надеюсь, это поможет другим учиться электронике, робототехнике, Arduino.

Обновленные файлы на GitHub: https://github.com/Mjrovai/MJRoBot-Line-Follower
Блог с уроками: MJRoBot.org

Приветствия с юга мира! Спасибо, Marcelo.

Код

Все фрагменты кода (snippets) и полные файлы доступны в разделе Code оригинального проекта и на GitHub.

Схемы

Файл схемы для Fritzing: oIDhLcHQ30lDgVBXZvb8.fzz