Михаил Момот

3-е издание

Санкт-Петербург
«БХВ-Петербург»
2023

УДК 007.52
ББК 32.816
М76

Момот М. В.

М76

Мобильные роботы на базе Arduino. — 3-е изд., перераб. и доп. — СПб.:
БХВ-Петербург, 2023. — 272 с.: ил. — (Электроника)
ISBN 978-5-9775-1703-4
Руководство для начинающих конструкторов написано в форме практических
проектов по построению мобильных роботов. Для их реализации выбрана популярная платформа Arduino и четырехколесная конструкция. Рассказано, как управлять моторами, осуществлять сборку механики и электроники, программировать
основные функции и управлять роботом. Роботы смогут обходить препятствия,
выбираться из запутанных лабиринтов.
В 3-м издании добавлены проекты по передаче изображения с камеры, удержанию направления движения по гироприборам, модель двухколесного балансирующего робота на моторах с энкодерами. Все сложные алгоритмы вынесены в отдельную библиотеку.
Электронный архив на сайте издательства содержит детали робота для печати
на 3D-принтере, векторные рисунки для резки лазером, листинги, дополнительные
библиотеки и программы.
Для читателей, интересующихся электроникой и робототехникой
УДК 007.52
ББК 32.816

Группа подготовки издания:
Руководитель проекта
Зав. редакцией
Редактор
Компьютерная верстка
Дизайн серии
Оформление обложки

Евгений Рыбаков
Людмила Гауль
Григорий Добин
Ольги Сергиенко
Марины Дамбиевой
Зои Канторович

"БХВ-Петербург", 191036, Санкт-Петербург, Гончарная ул., 20

ISBN 978-5-9775-1703-4

© ООО "БХВ", 2023
© Оформление. ООО "БХВ-Петербург", 2023

ОГЛАВЛЕНИЕ

Предисловие ..................................................................................................... 9
Глава 1. Основные составные части робота ............................................. 13
Информационно-измерительная система ..................................................................... 13 
Датчик касания .......................................................................................................... 14 
Датчик температуры ................................................................................................. 14 
Датчик освещенности ............................................................................................... 15 
Датчик препятствия .................................................................................................. 15 
Ультразвуковой датчик расстояния ........................................................................ 16 
Оптический рефлекторный датчик расстояния ..................................................... 16 
Детектор шума .......................................................................................................... 17 
Энкодер ..................................................................................................................... 17
Датчик движения ....................................................................................................... 18 
Датчик газа ................................................................................................................ 18 
Датчик влажности ..................................................................................................... 18 
Видеокамера ............................................................................................................. 19 
Система принятия решений ............................................................................................ 20 
Контроллер Arduino .................................................................................................. 20 
Система связи .................................................................................................................. 22
Инфракрасный приемник ......................................................................................... 22 
Канал Bluetooth ......................................................................................................... 23 
Канал Wi-Fi ................................................................................................................ 23 
Устройства отображения информации (дисплеи) ................................................. 24 
Исполнительная система ................................................................................................ 24 
Электрический двигатель постоянного тока .......................................................... 25 
Сервомотор ............................................................................................................... 26 
Шаговый двигатель .................................................................................................. 26 
Система энергоснабжения .............................................................................................. 27 
Механика робота .............................................................................................................. 27 
Элементы электрической схемы .................................................................................... 27 
Резистор .................................................................................................................... 27
Светодиод ................................................................................................................. 28 
Выключатель ............................................................................................................. 28 

4

Оглавление

Сервисные платы............................................................................................................. 28 
Arduino Sensor Shield v5.0 ........................................................................................ 28 
Макетная плата ......................................................................................................... 29 
Выводы ............................................................................................................................. 30

Глава 2. Провода и их соединения .............................................................. 31
Виды проводов ................................................................................................................. 31
Одножильные ............................................................................................................ 31 
Многожильные .......................................................................................................... 31 
Тип изоляции ............................................................................................................. 32 
Способы соединений проводов ...................................................................................... 32 
Скрутка ...................................................................................................................... 32
Разъемные соединения ........................................................................................... 33 
Пайка и ее основы .................................................................................................... 35 
Оборудование и материалы ............................................................................ 35 
Этапы пайки ...................................................................................................... 36 
Выбор паяльника .............................................................................................. 37 
Уход за паяльником ......................................................................................... 38 
Припои ............................................................................................................... 38 
Флюсы ............................................................................................................... 39 
Выводы ............................................................................................................................. 39
Глава 3. Электропитание............................................................................... 40
Закон Ома ......................................................................................................................... 40
Электрическая мощность ................................................................................................ 40 
Характеристики элементов питания .............................................................................. 40 
Номинальное напряжение ....................................................................................... 40 
Номинальный ток ...................................................................................................... 41 
Внутреннее сопротивление источника питания .................................................... 41 
Емкость батареи или аккумулятора ........................................................................ 41 
Форм-фактор ............................................................................................................. 41 
Типы элементов электрического питания ..................................................................... 41 
Солевые батареи ...................................................................................................... 42 
Алкалиновые батареи .............................................................................................. 42 
Никель-металлогидридные аккумуляторы ............................................................. 42 
Литий-ионные и литий-полимерные аккумуляторы ............................................... 42 
Стабилизация электропитания ....................................................................................... 43 
Стабилизация напряжения ...................................................................................... 44 
Стабилизация электрического тока ........................................................................ 46 
Измерение электрического тока, напряжения и сопротивления ................................. 46 
Защита от короткого замыкания по питанию ................................................................ 48 
Защита от неверной установки элементов питания (переполюсовки) ....................... 50 
Зарядка аккумуляторов на примере Li-ion и Li-pol ......................................................... 51 
Выводы ............................................................................................................................. 52
Глава 4. Основы программирования Arduino............................................ 53
Компьютерная программа ............................................................................................... 53 
Алгоритм ........................................................................................................................... 54
Среда разработки Arduino IDE........................................................................................ 55 
Установка Arduino IDE .............................................................................................. 55 

Оглавление

5

Начало работы с Arduino IDE .................................................................................. 56 
Подключение контроллера Arduino к ПК ................................................................ 56 
Мигаем светодиодом ....................................................................................................... 60 
Мониторинг работы программы ..................................................................................... 61 
Переменные ..................................................................................................................... 62
Условные операторы ....................................................................................................... 64 
Оператор if ... else ..................................................................................................... 64 
Оператор switch ... case ........................................................................................... 67 
Операторы циклов while и for.......................................................................................... 68 
Функции ............................................................................................................................. 70
Элементы объектно-ориентированного программирования ....................................... 71 
Разделение программы (внутренние библиотеки) ....................................................... 72 
Выводы ............................................................................................................................. 73

Глава 5. Ходовая часть ................................................................................. 74
Типы ходовых частей ...................................................................................................... 74 
Гусеницы ................................................................................................................... 74 
Колеса с дифференциалом ..................................................................................... 75 
Колеса на моторах .................................................................................................... 75 
Летающие роботы..................................................................................................... 76 
Выбор двигателей............................................................................................................ 76 
Драйверы двигателей ...................................................................................................... 78 
Сборка макета .................................................................................................................. 80
Управляем двигателем без Arduino ........................................................................ 80 
Широтно-импульсная модуляция ............................................................................ 82 
Подключаем контроллер Arduino ............................................................................ 83 
Подключаем библиотеку mobrob3.zip и пишем тестовую программу .................. 86 
Добавляем регулирование на основе ШИМ ........................................................... 87 
Тестовая программа управления двигателями
с регуляцией на основе ШИМ .................................................................................. 87 
Выводы ............................................................................................................................. 89

Глава 6. Сборка базовой модели ................................................................. 90
Минимальный комплект .................................................................................................. 90 
Двигатели ......................................................................................................................... 92
Система электропитания ................................................................................................ 93 
Подбираем компоненты .................................................................................................. 96 
Схемы ............................................................................................................................... 98
Проверка правильности подключения платы драйвера и двигателей ..................... 102 
Пример использования платы Arduino Sensor Shield ................................................. 103 
Установка устройств обратной связи ........................................................................... 104 
Укладка проводов .......................................................................................................... 107 
Выводы ........................................................................................................................... 107
Глава 7. Схема управления движением.................................................... 108
Переменные и функции управления моторами .......................................................... 108 
Подключение управления моторами .................................................................... 108 
Управление мощностью ......................................................................................... 108 
Движение в нужном направлении ......................................................................... 109 

6

Оглавление

Первая поездка .............................................................................................................. 109
Алгоритм .................................................................................................................. 109 
Программа ............................................................................................................... 110 
Сигнал светодиодом ...................................................................................................... 111 
Выводы ........................................................................................................................... 112

Глава 8. Дистанционное управление роботом ........................................ 113
Способы дистанционного управления ......................................................................... 113 
Управление роботом по каналу инфракрасной связи ................................................ 114 
Схема подключения ............................................................................................... 114 
Рекомендации по установке .................................................................................. 116 
Установка библиотеки IRremote ............................................................................ 117 
Получение кодов кнопок для используемого пульта ........................................... 117 
Программа ............................................................................................................... 120 
Управление роботом по каналу Bluetooth ................................................................... 123 
Подбор элементной базы ...................................................................................... 124 
Подключение к Arduino........................................................................................... 125 
Смена имени контроллера Bluetooth и робота .................................................... 126 
Настройка смартфона ............................................................................................ 127 
Устранение радиопомех ........................................................................................ 129 
Программа ............................................................................................................... 129 
Управляем роботом по Wi-Fi (ESP-01) ........................................................................ 133 
Выводы ........................................................................................................................... 138
Глава 9. Робот, держащий направление
по электронному гироскопу-акселерометру ............................................ 139
Гироскоп.......................................................................................................................... 139
Акселерометр ................................................................................................................. 141
Электронный гироскоп................................................................................................... 142 
Подключение гироскопа-акселерометра MPU-6050 ................................................... 143 
Получение данных с MPU-6050 .................................................................................... 145 
Шкала значений MPU-6050 ........................................................................................... 148 
Модернизация робота ................................................................................................... 148 
Монтаж устройства ................................................................................................. 148 
Программирование ................................................................................................. 148 
Основные функции для получения углов ..................................................... 148 
Стабилизация поворотов: ПИД-регулятор ................................................... 150 
Добавляем функцию движения ..................................................................... 153 
Выводы ........................................................................................................................... 157
Глава 10. Движение по черной линии ....................................................... 158
Обнаружение черной линии.......................................................................................... 159 
Фотодиод ................................................................................................................. 159 
Фоторезистор .......................................................................................................... 160 
Фототранзистор ...................................................................................................... 161 
Инфракрасный датчик отражения TCRT 5000 ..................................................... 161 
Подготовка робота: установка датчиков ...................................................................... 163 
Упрощенная программа движения ............................................................................... 166 
Используем аналоговые данные «на всю катушку» ................................................... 169 
Выводы ........................................................................................................................... 173

Оглавление

7

Глава 11. Измерение расстояния и поворотная голова ......................... 174
Измеряем расстояние ................................................................................................... 174 
Инфракрасный датчик расстояния GP2Y0A021YK0F ................................................ 175 
Лазерный датчик расстояния VL53L1X ........................................................................ 176 
Ультразвуковые дальномеры HC-SR04 и US-026/25 ................................................. 177 
Поиск препятствия ......................................................................................................... 179 
Поворотная голова ........................................................................................................ 184 
Схемы подключения ............................................................................................... 185 
Управление сервомашинкой ................................................................................. 187 
Монтаж головы ........................................................................................................ 189 
Тестовая программа «Ведение цели» .................................................................. 190 
Если что-то пошло не так... ........................................................................................... 191 
Выводы ........................................................................................................................... 192
Глава 12. Ходовые испытания: обход препятствий ............................... 193
Программа проверки и настройки основных функций робота ................................... 193 
Выводы ........................................................................................................................... 200
Глава 13. Робот, находящий выход из лабиринта .................................. 201
Способ обхода лабиринта ............................................................................................ 202 
Обход лабиринта без модернизации робота .............................................................. 203 
Программа ............................................................................................................... 207 
Выводы ........................................................................................................................... 213
Глава 14. Робот с видеокамерой ............................................................... 214
Аналоговое видео с робота .......................................................................................... 214 
Цифровое видео для робота: ESP32-CAM .................................................................. 216 
Робот с камерой ESP32-CAM ....................................................................................... 218 
Добавление контроллеров ESP в среду ArduinoIDE ........................................... 219 
Прошивка ESP32-CAM ........................................................................................... 221 
Настройка программного обеспечения робота ........................................................... 222 
Сборка электроники ....................................................................................................... 224 
Сборка механики ............................................................................................................ 224 
Выводы ........................................................................................................................... 226
Глава 15. Робот и пройденное расстояние .............................................. 227
Моторы с инкрементальным энкодером ...................................................................... 227 
Как работает инкрементальный энкодер? ........................................................... 228 
Получение информации о вращении с инкрементальных энкодеров ............... 229 
Следим за энкодерами в приложении .................................................................. 236 
Выводы ........................................................................................................................... 243
Глава 16. Балансирующий робот............................................................... 244
Теория ............................................................................................................................. 244
Как найти угол наклона робота? ........................................................................... 246 
Регулятор балансировки робота ........................................................................... 247 
Программирование ........................................................................................................ 248 
Порядок подбора коэффициентов пропорциональности регулятора ............... 254 
Выводы ........................................................................................................................... 260
Приложение. Описание электронного архива ......................................... 261
Предметный указатель................................................................................ 265

ПРЕДИСЛОВИЕ

Здравствуй, дорогой читатель!
Хотя я не уверен, что правильно называть того, кто читает эту книгу, просто читателем, — скорее, он конструктор или изобретатель. Потому что только лишь
чтение книги без сопутствующей сборки и программирования роботов не столь
интересно.
Представляю вам третье издание книги «Мобильные роботы на базе Arduino». Книга традиционно про мобильных роботов и традиционно не сильно повторяет предыдущее издание. Предыдущие издания доступны в виде электронных книг,
поэтому в том, что третье издание не копирует основные алгоритмы и схемы из
предыдущих, беды нет.
Эту книгу я составлял как руководство для начинающих конструкторов, т. е.
людей, которым нравится конструировать. А за основу взял конструирование несложных роботов на весьма популярной в настоящее время платформе Arduino.
Arduino же выбрал потому, что проекты, выполненные на ее основе, весьма простые и функциональные. Платформа Arduino открытая, а это значит, что изготавливать дополнительные модули для нее может любой человек или организация, то же
относится и к программам.
Изучив книгу, вы получите знания по конструированию и программированию мобильных роботов на платформе Arduino. Поймете принципы действия датчиков,
при помощи которых роботы следят за внешним миром. Будете собирать своих
оригинальных роботов и удаленно управлять их работой.
Книга разбита на главы.
 Главы с 1-й по 4-ю являются вводными — они дают основу, подготавливают вас

к конструированию робота. Так, глава 1 посвящена составным частям, из которых может быть сделан робот. Глава 2 учит выполнять электрические соединения. Глава 3 — рассказывает об источниках электрического питания, их стабилизации и выборе подходящего источника питания для робота. В главе 4 даны

10

Предисловие

основы программирования контроллеров Arduino, поскольку начинать программировать можно еще до установки контроллера в робота.
 Главы с 5-й по 8-ю посвящены сборке базовой конструкции робота. Так, глава 5

содержит обзор ходовых частей и учит основам управления двигателями.
Глава 6 дает практические навыки по сборке ходовой части. Глава 7 представляет основы управления движениями робота — в ней разрабатывается первая,
базовая, программа, содержащая блоки управления его движениями. Глава 8
посвящена способам дистанционного управления роботом со смартфона на
Android или с пульта дистанционного управления, подобного телевизионному
пульту.
 Главы с 9-й по 14-ю вводят в проектирование конкретных роботов на собранной

платформе, которые должны, выполняя определенные действия, добиваться поставленных перед ними разнообразных целей. Так, глава 9 посвящена ориентированию робота в пространстве при помощи специальных электронных приборов: гироскопа и акселерометра — робот научится ехать прямо, не сбиваясь
с указанного направления и точно поворачивать на заданный угол. Глава 10 раскрывает особенности работы с датчиком черной линии и научит вас регулировать поведение робота так, чтобы он следовал по линии в нужном направлении.
Глава 11 посвящена измерению расстояний различными датчиками и использованию поворотных сервомашинок с установленными на них датчиками для
измерения расстояния с разных направлений. Главы 12 и 13 дадут понимание
программной логики, которая позволяет роботу объезжать препятствия и не заблудиться в запутанном лабиринте. В главе 14 робот оснащается видеокамерой,
что дает возможность следить за его работой его же «глазами». В ней рассмотрен вариант камеры с аналоговым видеопередатчиком, а также цифровая камера,
которая формирует точку доступа Wi-Fi, что позволяет через браузер подключаться к роботу со смартфона, просматривать видео с камеры и одновременно
управлять роботом.
 В главах 15 и 16 в конструкцию робота вносятся значительные изменения. Робот

получает новые редукторные моторы с энкодерами, что дает ему возможность
измерять свою скорость и пройденное расстояние. В главе 15 мы научим робота
точно управлять скоростью своего движения и пройденным расстоянием. В главе 16 робот научится двигаться и держать равновесие в вертикальном положении на двух колесах, и вы получите знания по регулированию этого процесса
самостоятельно в своих проектах балансирующих роботов.
Книга поможет вам разобраться в работе основных датчиков: ультразвукового датчика расстояния, детектора препятствия, датчика цвета (черный или белый), электронного гироскопа, энкодеров — чтобы вы смогли обеспечить своим роботам требуемый функционал. В книге также приведены советы по самостоятельному изготовлению некоторых датчиков.
Электронный архив с материалами к этой книге можно скачать с сервера издательства «БХВ» по ссылке https://zip.bhv.ru/9785977517034.zip или со страницы
книги на сайте https://bhv.ru/ (см. приложение). В электронный архив включена

Предисловие

11

библиотека mobrob3.zip — ее требуется стандартным путем установить в среду
Arduino IDE, после чего файлы библиотеки будут доступны для работы, а в списке
примеров среды Arduino IDE появятся все листинги из книги — они разбиты по
главам и имеют четкое обозначение: «номер главы_пояснение_номер листинга
в главе». Кроме библиотеки программ, в архиве содержатся файлы с 3D-моделями
корпусов и колес роботов, которые использовались при написании глав и создании
скетчей и которые пригодны для самостоятельной печати деталей роботов из книги
на 3D-принтере.
Я уверен, что изучение книги и конструирование роботов будет вам не только полезно с точки зрения получения новых знаний и умений, но и станет увлекательным и интересным занятием.
С уважением, Михаил Момот

ГЛАВА 1
ОСНОВНЫЕ СОСТАВНЫЕ ЧАСТИ
РОБОТА

Что мы имеем в виду, когда произносим слово «робот»? В основном подразумевается некое техническое творение, которое либо самостоятельно, либо посредством
удаленного управления может совершать определенные действия.
Слово «робот» впервые применил для обозначения искусственных людей чешский
писатель Карел Чапек в своей пьесе «Россумские универсальные роботы». К. Чапек
называл так искусственные создания, внешне не отличающиеся от обычных людей,
но безгранично преданные человечеству и поэтому безмерно эксплуатируемые. Современные роботы, это, конечно, не искусственные люди, но весьма продвинутые
автоматические механизмы, способные на очень многое благодаря наличию у них
развитой структуры компонентов, отвечающих за разнообразные действия.
Внутренняя начинка роботов не сильно отличается от обычного персонального
компьютера. Но, имея другие цели, роботы выглядят иначе. Любого робота можно
условно разделить на следующие части:
 информационно-измерительная система;
 система принятия решений (система внутреннего управления);
 система связи;
 исполнительная система;
 система энергоснабжения;
 механика робота (скелет).

Информационно-измерительная система
Информационно-измерительная система — это совокупность органов чувств робота. Она отвечает за восприятие роботом внешнего мира. Элементы информационно-измерительной системы называют датчиками или сенсорами. Самым простым

14

Глава 1

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

Датчик касания
Рассмотрим наиболее распространенные органы чувств робота более подробно. На
рис. 1.1 представлены датчик касания и его принципиальная схема. Датчик реализован в виде нефиксируемой кнопки с рычажком, при нажатии на который одна
пара контактов замыкается, а другая размыкается.

Рис. 1.1. Датчик касания

Датчик температуры
На рис. 1.2 показан датчик температуры — принцип его работы основан на изменении сопротивления компонента электрической схемы при изменении температуры,
что приводит к изменению выходного напряжения, которое и служит для замера
температуры.

Основные составные части робота

15

Рис. 1.2. Датчик температуры

Датчик освещенности
На рис. 1.3 изображен пороговый датчик освещенности, который срабатывает по
достижении освещенностью определенного значения. Пороговое значение срабатывания датчика можно регулировать. Основой датчика служит фоторезистор, сопротивление которого изменяется при изменении освещенности.

Рис. 1.3. Датчик освещенности

Датчик препятствия
Датчик препятствия (рис. 1.4) состоит из инфракрасного светодиода, излучающего
в невидимом человеку инфракрасном диапазоне, и фототранзистора. Светодиод
светит на препятствие, фототранзистор реагирует на отраженный свет, а величина
тока фототранзистора зависит от уровня освещенности препятствия. За счет подстройки можно изменять пороговый уровень срабатывания датчика и тем самым
менять расстояние срабатывания. Недостатком датчика является реакция на сторонние источники инфракрасного света.

16

Глава 1

Рис. 1.4. Датчик препятствия

Ультразвуковой датчик расстояния
Более сложным, но и более точным, чем инфракрасный датчик препятствия, является ультразвуковой датчик расстояния (рис. 1.5). Он излучает ультразвуковую
волну и принимает отраженный сигнал, а расстояние определяется по времени
распространения сигнала до препятствия и обратно. При этом точность датчика
составляет несколько миллиметров. Однако на определение расстояния требуется
некоторое время, которое робот должен провести неподвижно.

Рис. 1.5. Ультразвуковой датчик расстояния

Оптический рефлекторный датчик расстояния
Интересным устройством является оптический рефлекторный датчик расстояния,
иногда также называемый инфракрасным датчиком расстояния (рис. 1.6). В основе его работы лежит триангуляционный принцип измерений.

Рис. 1.6. Оптический рефлекторный датчик расстояния

Датчик состоит из передатчика, излучающего прямо вперед, и приемника, отстоящего от него на известном (базовом) расстоянии. Вместе с отражающим объектом
они образуют прямоугольный треугольник. Чем дальше объект, тем больше угол

Основные составные части робота

17

в основании прямоугольника. Зная угол и базу, легко вычислить расстояние тригонометрически.
Следует заметить, что датчик имеет нелинейный выход — при изменении расстояния до объекта сигнал на аналоговом выходе датчика изменяется непропорционально, поэтому для расчета применяется специальная формула.

Детектор шума
На рис. 1.7 показан детектор шума на конденсаторном микрофоне, срабатывающий
на заданный уровень звука. Порог его срабатывания можно плавно регулировать.

Рис. 1.7. Детектор шума

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

Рис. 1.8. Энкодер

18

Глава 1

Датчик движения
Датчик движения (рис. 1.9) состоит из приемника, реагирующего на инфракрасный
свет, и линзы Френеля, обеспечивающей большие углы обзора. Конструкция датчика такова, что он не срабатывает на излучение от неподвижных объектов, но срабатывает, когда объект начинает двигаться.

Рис. 1.9. Датчик движения с линзой Френеля

Датчик газа
Датчик газа (рис. 1.10) реагирует на увеличение концентрации углекислого и угарного газов. Его можно использовать при разработке пожарной сигнализации. Если
в помещении увеличивается концентрация угарного газа, человек этого не почувствует, а датчик сработает и предупредит о грозящей опасности.

Рис. 1.10. Датчик угарного газа

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

Основные составные части робота

19

Рис. 1.11. Датчик влажности

Видеокамера
Информационно-измерительная система роботов может включать видеокамеру,
которая состоит из проецирующей линзы и фоточувствительной матрицы (рис. 1.12).
Изображение проецируется на матрицу, каждый элемент матрицы генерирует сигнал, соответствующий цвету и освещенности определенной точки объекта. Полученная информация, закодированная числами (в случае цифрового сигнала), передается на обработку. Следует отметить, что контроллеры Arduino, используемые
в простых проектах, не подходят для обработки видео и фото. Фотокадр размером
640 на 480 точек при 256 значениях освещенности (для черно-белого кадра) потребует 300 килобайт оперативной памяти, чего в большинстве контроллеров Arduino
попросту нет.

Рис. 1.12. Видеокамера

20

Глава 1

Система принятия решений
Информация, полученная от датчиков, должна быть обработана и проанализирована — этим занимается система принятия решений (внутреннего управления). Она
содержит правила поведения робота в зависимости от времени и информации, поступающей от других систем. Включение робота приводит к тому, что он начинает
действовать согласно этим правилам. Под действием понимается управление своими механизмами, включение или отключение моторов, передача и получение информации по системам связи.
Правила поведения роботов содержатся в его памяти в машинных кодах. Но перед
этим они создаются людьми сначала в виде алгоритмов, а затем в виде программы
на языке программирования. Роботы сами не способны изменять правила своего
поведения.
Система принятия решений является «компьютером» робота и может быть реализована на схожих с ним компонентах. Эту систему можно также назвать центральной нервной системой робота.
Функции системы принятия решений в роботах выполняют микроконтроллеры.
Микроконтроллер — это микросхема, включающая в себя несколько устройств:
 процессор осуществляет все логические и арифметические операции и управляет
выполнением программы робота;
 постоянная память (постоянное запоминающее устройство) выполняет роль
«жесткого диска» робота, хранит программы и данные, не стирается при выключении питания;
 оперативная память (оперативное запоминающее устройство) является памятью
с быстрым доступом и используется контроллером при работе для хранения
программ и данных;
 аналого-цифровые преобразователи преобразуют уровни напряжения в числовую форму;
 широтно-импульсные генераторы предназначены для генерации электрических
импульсов с определенной частотой и шириной и служат для управления внешними устройствами — например, скоростью вращения двигателя постоянного
тока.
Микроконтроллеры (рис. 1.13) функционально схожи с современными компьютерами, но гораздо слабее их. Частота работы микроконтроллера: 8–32 МГц, размер
оперативной памяти: 1–16 Кбайт, емкость постоянной памяти: 1–32 Кбайт.

Контроллер Arduino
Проект, который существенно упростил разработку несложных роботов, зародился
в Италии в 2005 году и благодаря открытой документации распространился по
всему миру. Его разработчики назвали свое устройство Arduino.
Arduino — это не только плата, содержащая микроконтроллер, стабилизаторы питания и удобно расположенные контактные площадки для подключения внешних

Основные составные части робота

21

Рис. 1.13. Микроконтроллер ATmega328

Рис. 1.14. Плата контроллера Arduino UNO на основе микроконтроллера ATmega328P

устройств. В проект Arduino входит простая и оригинальная программная оболочка, позволяющая относительно легко создавать и модифицировать программы,
а также огромное количество программных библиотек для подключения датчиков,
моторов и других устройств.
Фактически Arduino — это электронный конструктор. На рис. 1.14 представлен
контроллер Arduino UNO. Номерами 0–13 обозначены цифровые порты, A0–A5 —
порты, которые могут анализировать уровень сигнала и преобразовывать его

22

Глава 1

в числовую форму. На плату может быть подано напряжение от 6 до 12 вольт, при
этом стабилизаторы питания преобразуют его в требуемые уровни.

Система связи
Когда требуется скорректировать работу робота либо получить дополнительную
информацию о его состоянии, следует использовать систему связи.
Система связи служит для организации взаимодействия робота с другими устройствами и «хозяином». Вмешательство «хозяина» требуется, когда автономная (самостоятельная) работа невозможна или нецелесообразна. Так, робот может попасть
в условия, правила действия в которых не описаны в его программе, или условия
требуют принятия решения человеком, или робот жестко запрограммирован так,
что любые действия совершаются только после получения команды от «хозяина».
Системы связи могут использовать следующие каналы:
 проводные каналы, когда робот соединен с пультом управления проводами;
 радиоканалы, когда информация передается радиоволнами;
 инфракрасные каналы, когда информация передается световыми импульсами;
 аудиоканалы, когда робот напрямую воспринимает речевые команды человека
и сам может сообщить ему некоторую информацию;
 визуальные каналы, когда сообщения выводятся на различные информационные
мониторы робота.

Инфракрасный приемник
Управляющие команды можно посылать на инфракрасный (IR) приемник, установленный на роботе (рис. 1.15). Это удобно, когда команд немного, и между вами
и роботом нет преград. Так, например, можно управлять движением робота.

Рис. 1.15. IR-приемник на плате (слева); IR-пульт (в центре); схема выводов (справа)

23

Основные составные части робота

Канал Bluetooth
Удобный способ общения с роботом предоставляет канал Bluetooth. Существует
масса программ для управления роботами по этому каналу с помощью смартфонов
на операционной системе Android. Модули Bluetooth для роботов (рис. 1.16) также
не являются редкостью. В одной из практических глав мы рассмотрим этот вопрос
подробнее.

Рис. 1.16. Приемопередатчик Bluetooth

Канал Wi-Fi
Радиоканалы связи не ограничиваются
только Bluetooth, существуют и широко
применяются модули Wi-Fi (рис. 1.17), при
помощи которых роботами можно управлять посредством беспроводных компьютерных сетей. Роботы при этом становятся
частью большой компьютерной системы.

Рис. 1.17. Приемопередатчик Wi-Fi

24

Глава 1

Устройства отображения информации (дисплеи)
К средствам связи можно также отнести и различные виды дисплеев для вывода
информации. Основная проблема подключения большинства дисплеев к Arduino
заключается в большом количестве задействованных контактов (портов ввода/вывода), но дисплеи (рис. 1.18), использующие I2C-шину, этой проблемы лишены — для их подключения нужно только питание и 2 контакта управления.

Рис. 1.18. Дисплей с управлением по шине I2C

Исполнительная система
Исполнительная система отвечает за все движения робота и определяет возможности робота выполнять другие физические действия. К исполнительной системе
относятся манипуляторы, ходовая часть и устройства позиционирования рабочих
элементов. Основой большинства исполнительных механизмов являются электрические двигатели (рис. 1.19).

Рис. 1.19. Электрические двигатели

Основные составные части робота

25

Электрический двигатель постоянного тока
Электрические двигатели различаются мощностью и скоростью вращения (количеством оборотов в минуту). Выбирать электрический двигатель нужно исходя из
конкретной задачи. Если мощность мала, применяют редукторные передачи, которые снижают количество оборотов на выходном валу, но увеличивают мощность.
На рис. 1.20 представлен двигатель постоянного тока с установленным на нем
редуктором с передаточным числом 48.

Рис. 1.20. Двигатель постоянного тока с редуктором

Редукторные передачи этого двигателя показаны на рис. 1.21. Вал двигателя вращается быстро, а колеса робота — в 48 раз медленнее, но в 48 раз мощнее. Это
позволяет роботу преодолевать препятствия и двигаться в гору. Конечно, размер
колеса также имеет значение.

Рис. 1.21. Редуктор двигателя

26

Глава 1

Сервомотор
Другим распространенным видом привода для робота является сервомотор (рис. 1.22).
Его отличие от двигателя постоянного тока с установленным редуктором в том, что
основная задача сервомотора — обеспечить определенный угол поворота вала,
который задается управляющим сигналом. Сервомоторы применяются в манипуляторах, поворотных механизмах, при создании шагающих роботов — везде, где требуется точность позиционирования угла поворота.

Рис. 1.22. Сервомотор

Внутри сервомотора, как правило, установлен двигатель постоянного тока и понижающий редуктор. Там же могут находиться схема управления и энкодер. На
рис. 1.23 изображен разобранный сервомотор (сервопривод).

Рис. 1.23. Сервомотор: двигатель постоянного тока (слева); энкодер (в центре); редуктор (справа)

Шаговый двигатель
Еще одним распространенным видом приводов является шаговый двигатель
(рис. 1.24). Он поворачивает свой вал небольшими дискретными значениями —
«шагами». Шаговые двигатели широко используются в станках с числовым программным управлением для организации точного движения обрабатывающего инструмента.

27

Основные составные части робота

Рис. 1.24. Шаговый двигатель: плата управления (слева); собственно шаговый двигатель (в центре);
схема (справа)

Система энергоснабжения
Система энергоснабжения несложного робота основана на электричестве. В нее
входят элементы питания, стабилизаторы питания, различные генераторы электроэнергии — например, солнечные батареи. Электропитанию роботов посвящена глава 3 книги.

Механика робота
Механика робота, его скелет, представляет собой совокупность элементов конструкции, к которым крепятся и с которыми соединяются все рассмотренные ранее
составляющие. Конструкция колес, ног, корпуса робота определяет его внешний
вид и во многом функциональность. Естественно, что для робота, выполняющего
физические движения, механика не менее, а нередко и более сложна, чем электроника. В главе 5 мы рассмотрим ходовую часть робота.

Элементы электрической схемы
Кроме уже рассмотренных компонентов, в схемах могут встречаться простые элементы — например, резисторы, светодиоды и пр.

Резистор
Резистор (рис. 1.25) играет роль ограничителя
тока и роль дополнительной нагрузки.

Рис. 1.25. Резистор

28

Глава 1

Светодиод
Светодиод (рис. 1.26) служит для подсветки
объектов и индикации.

Рис. 1.26. Светодиод

Выключатель
Выключатель, или тумблер (рис. 1.27), служит для включения/выключения робота.
Выключатели ставятся в цепь электропитания робота, разрывая соединение положительного контакта с элементом питания.

Рис. 1.27. Выключатель

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

Arduino Sensor Shield v5.0
Примером подобной платы в Arduino-проектах может служить Arduino Sensor
Shield v5.0 (рис. 1.28). Эта плата устанавливается сверху в разъемы платы Arduino

Основные составные части робота

29

UNO или Arduino MEGA и обеспечивает удобное подключение таких устройств,
как: дисплей 128×64 с последовательным (SPI) или параллельным портом, сервомоторы, модуль Bluetooth, модуль беспроводной связи APC220, а также устройств
с последовательным интерфейсом и с интерфейсом I2C. Списком указанных устройств возможности платы не ограничиваются (подробное описание этой и других
интересных сервисных плат не трудно отыскать, например, на сайте arduino.cc).

Рис. 1.28. Сервисная плата Arduino Sensor Shield v5.0

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

Макетная плата
Удобным для временного монтажа может стать применение макетной платы
(рис. 1.29, а). Такая плата предназначена для быстрого монтажа экспериментальных схем. Она позволяет, буквально на коленках, собрать электронное устройство,
имеющее множество соединений. На рис. 1.29, б показано соединение контактов
внутри этой платы.

30

Глава 1

а

б
Рис. 1.29. Макетная плата (а) и схема ее внутренних соединений (б)

Выводы
В краткой форме мы рассмотрели здесь основные системы робота: информационно-измерительную, принятия решений, связи, исполнительную, энергоснабжения, а
также механику робота и вспомогательные элементы. Особенности работы с ними
будут раскрыты более подробно в процессе выполнения соответствующих проектов. Следующая же глава посвящена проводам робота и их соединению.

ГЛАВА 2
ПРОВОДА И ИХ СОЕДИНЕНИЯ

Виды проводов
Электрические элементы робота соединяются друг с другом посредством проводников электрического тока. Если элементы расположены на специальной подложке — плате, то проводниками являются ее медные «дорожки». Если элементы разнесены, используют провода. Провода различают по типу металла, из которого они
состоят, по толщине (сечению) и количеству жил. Для роботов используют медные
провода.
Медные провода являются основой соединений в бытовой электронике из-за малого сопротивления электрическому току, хорошей теплопроводности и гибкости,
а также возможности качественной пайки.

Одножильные
Одножильные провода (рис. 2.1) могут применяться в соединениях, которые не
будут подвергаться смещениям относительно друг друга. Если такие провода перегибать, то они быстро ломаются и перестают работать.

Многожильные
Многожильные провода (рис. 2.2) отличаются тем, что хорошо переносят изгибы
(не ломаются), благодаря чему используются в роботостроении для соединения
подвижных частей.

Рис. 2.1. Одножильный провод

Рис. 2.2. Многожильный провод

32

Глава 2

Тип изоляции
Изоляция проводов выполняется для предотвращения короткого замыкания и
должна учитывать вид и величину напряжения и тока, который протекает по проводам, а также с какой целью провода используются. Самая распространенная для
невысоких напряжений — изоляция ПВХ. Ее недостаток — низкая температура
плавления, что может приводить к разрушению изоляции при пайке. Более надежна
силиконовая изоляция — провода с силиконовой изоляцией рекомендуются для
новичков и тех, кто учится паять, т. к. силиконовая изоляция при пайке проводов не
портится.

Способы соединений проводов
Так как создание робота подразумевает самостоятельное соединение проводов, то
разобраться, как это можно сделать качественно, будет не лишним. Рассмотрим
основные способы электрических соединений, которые можно применять при конструировании роботов.

Скрутка
Соединение скруткой (рис. 2.3) применяется тогда, когда требуется создать контакт
временно, или условия, в которых вы работаете, не позволяют применить другой
тип соединений. Для соединения скруткой потребуются бокорезы или нож. Без нажима зажимаем бокорезами изоляцию проводов в месте, по которое нужно снять

Рис. 2.3. Этапы соединения скруткой: провода и бокорезы (слева); зачищенные и распушенные
провода (в центре вверху); скрутка (в центре внизу); изолирование скрутки (справа вверху);
изолированная скрутка (справа внизу)

Провода и их соединения

33

изоляцию, и дергаем. Если все сделано правильно, изоляция оторвется, а жилы
проводов останутся нетронутыми. Распушим жилы и скрутим провода друг с другом. Место скрутки обязательно нужно заизолировать. Следует помнить, что медь
очень быстро окисляется, покрываясь непроводящей электрический ток пленкой,
что через некоторое время сделает соединение неработоспособным. При первой
возможности соединение скруткой нужно пропаять.

Разъемные соединения
Разъемные соединения широко применяются при быстром монтаже радиодеталей,
но провода с разъемными клеммами высокого качества стоят дорого, а дешевые не
лишены недостатков, что, впрочем, компенсируется скоростью монтажа. Недостатки разъемных соединений заключаются в окислении контактов со временем и потере соединения, а также в разрыве соединения при небольшом случайном усилии.
Платы Arduino содержат клеммные разъемы, подобными разъемами оснащается и
большинство дополнительных электронных модулей (датчики, драйверы двигателей). На рис. 2.4 представлены клеммы типа Dupont.

Рис. 2.4. Разъемные соединения

Часто разъемные соединения применяют совместно с платами расширения
(рис. 2.5) или с макетной платой (рис. 2.6). Это позволяет быстро создать прототип
и проверить его работоспособность, что очень важно при конструировании.
Для соединения проводов, питающих устройства большой мощности, — например,
двигателей, применяются клеммы с болтовым зажимом, которые почти не уступают соединениям пайкой.

34

Рис. 2.5. Сервомотор соединен с платой расширения при помощи разъема

Рис. 2.6. Использование макетной платы для сборки прототипа

Глава 2

Провода и их соединения

35

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

Оборудование и материалы
Нам потребуются:
 паяльник;
 припой;
 паяльный флюс или канифоль.
Для комфортной работы еще будут нужны:
 острый перочинный нож или бокорезы;
 стальная губка для быстрой очистки жала паяльника от гари;
 пинцет;
 подставка для паяльника.
Набор оборудования и материалов для пайки приведен на рис. 2.7.

Рис. 2.7. Принадлежности для пайки: катушка с припоем (1); проволочный припой с сердцевиной
из канифоли (2); губка для очистки жала паяльника (3); пинцет (4); канифоль (5); паяльник (6);
бокорезы (7); острый нож (8); плоскогубцы (9)

36

Глава 2

Этапы пайки
Пайка проводов (рис. 2.8) состоит из следующих этапов:
1. Зачищаем бокорезами кончики проводов от изоляции. Для многожильного провода скручиваем жилы в одну косичку.

1

3

2

4

5

Рис. 2.8. Процесс пайки: обрезка края проводов (1); зачистка (2); лужение (3);
готовые к пайке концы (4); спаянные провода (5)

Провода и их соединения

37

2. Лужение провода. Если попробовать спаять нелуженые провода, то, скорее всего, пайка будет некачественной, потому что на поверхности медных жил находится окись, препятствующая прилипанию припоя. Для лужения помещаем кончик жала паяльника в канифоль и, пока расплавленная канифоль не испарилась
с жала, переносим ее на зачищенный кончик провода. Теперь повторяем ту же
операцию с припоем: берем на кончик паяльника каплю припоя, а затем переносим ее на кончик провода. Капля припоя должна растечься, и оголенный кончик
провода приобретет цвет припоя, при этом жилки провода прочно спаяются между собой. Операция лужения перед пайкой обязательна.
3. Соединяем спаиваемые концы проводов между собой и дотрагиваемся паяльником с капелькой припоя до места соединения — капля растечется по проводам и,
после охлаждения, прочно спаяет провода. Долго паяльник держать не нужно —
для тонких проводов достаточно долей секунды.
4. После охлаждения место спайки необходимо изолировать изолентой или трубкой ПВХ. Начинающие небрежно относятся к изоляции, что в дальнейшем
может привести к порче электроники вашего проекта.

Выбор паяльника
Выбор паяльника зависит от цели его использования. Для удобной пайки тонких
проводов и контактов электронных схем требуется паяльник с тонким жалом и
мощностью 20–30 ватт. Я рекомендую приобрести отечественный паяльник с медным жалом (рис. 2.9).

Рис. 2.9. Паяльник с медным жалом

Конечно, он может оказаться дороже импортного паяльника с подобными характеристиками, но намного точнее по параметрам. Однажды я приобрел два одинаковых импортных 20-ваттных паяльника, но один из них имел мощность, соответствующую номиналу, а другой постоянно перегревался, и им невозможно было паять.

38

Глава 2

Впрочем, высококачественные паяльники зарубежного производства с регулировкой температуры, не обгорающими жалами и набором сменных жал разного вида
стоят достаточно дорого.
Если вы планируете паять толстые провода, то потребуется паяльник мощностью
40–60 ватт, имейте только в виду, что тонкие провода и микросхемы паять им неудобно.

Уход за паяльником
При пайке на жале паяльника постоянно скапливается нагар, что препятствует смачиванию его припоем. Нагар можно убрать, поместив жало в проволочную губку.
Такую губку можно приобрести в магазине кухонных принадлежностей, но есть и
специализированные — в них губка помещена в специальную баночку (рис. 2.10).

Рис 2.10. Проволочная губка для снятия нагара

Медное жало может обгорать до такой степени, что чистка в губке уже не помогает, — в этом случае кончик жала зачищают напильником. Напильником также
можно придавать кончику жала удобную для пайки форму. Когда жало полностью
сработается, его можно заменить — запасные жала продают в магазинах электроники.

Припои
Выбор припоя также важен. В качестве припоя применяется легкоплавкий сплав
металлов, температура плавления которого может колебаться от 200 до 350 градусов. При монтаже электроники используются оловянно-свинцовые припои. Согласно отечественной маркировке, оловянно-свинцовый пропой обозначается буквами
ПОС, после которых идет число, обозначающее процентное содержание олова.
Температуры плавления различных оловянно-свинцовых припоев в градусах Цельсия: ПОС30 — 240°, ПОС40 — 210°, ПОС61 — 180°, ПОС90 — 310°.
Проволочный припой может содержать внутри себя сердцевину из канифоли для
лучшего качества спайки.

Провода и их соединения

39

Флюсы
Флюс служит для удаления слоя окисла со спаиваемых поверхностей, предохраняет
металл от окисления при пайке, а также способствует равномерному растеканию
припоя. Флюсы делятся на активные и неактивные. Активные флюсы содержат активные кислоты и применяются для пайки различных металлов, а не только меди.
Остатки активных флюсов должны обязательно удаляться после пайки смывкой,
иначе они начинают разъедать место пайки. Неактивные флюсы в удалении не нуждаются, но справляются далеко не со всеми окислами металлов.
К неактивным флюсам относится канифоль и ее растворы. Для большинства электронных контактов она подойдет — смачиваем контакт канифолью, а затем паяем
припоем.
Но в случаях, когда спаять требуется сталь или алюминий, канифоль бесполезна.
Так, если надо припаять провод к контакту аккумулятора, без активного флюса не
обойтись. Активные флюсы (рис. 2.11) бывают жидкими и желеобразными. Желеобразными флюсами пользоваться удобнее — они не стекают после нанесения на
поверхность. Самым распространенным активным паяльным флюсом является
паяльная кислота, и ее обязательно нужно смывать после пайки. Хорошо зарекомендовал себя флюс для алюминия, с ним можно паять почти все металлы.

Рис. 2.11. Активные флюсы

Выводы
Мы рассмотрели здесь виды проводов и соединений, а также — подробно — этапы
и особенности пайки, ведь пайка является наиболее качественным соединением для
электрических проводов. Если вопросы все же остались, можно обратиться к источникам в сети Интернет или найти профессиональную литературу по пайке.
Следующая глава будет посвящена электрическому питанию роботов, выбору источников электропитания и их особенностям.

ГЛАВА 3
ЭЛЕКТРОПИТАНИЕ

Прежде чем начать обсуждать источники электрического питания, которые можно
использовать в роботах, следует вспомнить немного теории.

Закон Ома
Электрический ток измеряется в амперах (А) и в формулах обозначается буквой I.
Электрическое напряжение измеряется в вольтах (В), его принято обозначать
буквой U. Сопротивление измеряется в омах (Ом) и обозначается буквой R.
Связь между ними записывается в виде формулы (закона) Ома:
I=

U
R

Электрическая мощность
При обсуждении источников электрического тока будет важна еще одна их характеристика — электрическая мощность:
P =U ⋅ I
Мощность измеряется в ваттах (Вт) и показывает работу, выполненную за 1 секунду электрическим током. Для нас весьма важна потребляемая роботом мощность.
На основании значений потребляемой мощности должен подбираться источник
электрического питания.

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

Электропитание

41

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

Внутреннее сопротивление источника питания
Реальные источники электропитания при нагрузке (подключении потребителя) нередко начинают греться. Причина этого явления в том, что сам источник питания
обладает небольшим, как правило, сопротивлением, которое называется внутренним. При прохождении тока источник начинает нагреваться как обычное сопротивление и при высоких температурах выходит из строя.
Чем меньше внутреннее сопротивление, тем больший ток мы можем держать, тем
более мощные моторы можно подключать. Например, для мощных моторов квадрокоптеров требуются аккумуляторы с внутренним сопротивлением 0,006–0,012 Ом.
Впрочем, для мобильных роботов подойдут и источники питания с большим сопротивлением — до 0,1 Ом.

Емкость батареи или аккумулятора
Для аккумуляторной батареи важна ее емкость, измеряемая в ампер-часах. Обозначают ее на аккумуляторах в миллиампер-часах или mAh.
Двигатели, которые будут применяться в рассматриваемых моделях роботов, требуют напряжения питания от 4 до 7 В и потребляют ток 0,1 А. Максимальный ток
при четырех одновременно работающих моторах будет тогда 0,4 А. Электронный
контроллер Arduino потребляет порядка 0,05 А. Так что при питании от источника
7 вольт будет потребляться мощность около 3 Вт, а общий ток достигнет 0,45 А.
Если емкость используемой батареи 2 ампер-часа (2000 mAh), то робот проработает
около 4,5 часа без остановки. Когда робот прекращает движение, потребление им
энергии снижается до потребления только контроллером.

Форм-фактор
Самыми распространенными типоразмерами элементов питания являются стандарты AA и AAA с размерами 14,5×50×5 и 10,5×44,5 мм соответственно. С появлением
литий-ионных аккумуляторов стал распространяться типоразмер 18650 (18×66,5 мм).
Указанные элементы питания представляют собой цилиндры с полюсами на концах. Существуют и другие форм-факторы элементов питания, но при построении
несложных роботов с использованием контроллеров Arduino чаще всего используются батареи AA и 18650.

Типы элементов электрического питания
Элементы питания можно поделить на перезаряжаемые и неперезаряжаемые. Неперезаряжаемые используются от начала эксплуатации до окончания нормальной
работы (обеспечение номинального напряжения и тока), после чего утилизируются.

42

Глава 3

Перезаряжаемые элементы принято называть аккумуляторами — они заряжаются
специальными зарядными устройствами, накапливают электрический заряд, затем
используются, а после использования и разряда снова заряжаются. Так как моторные роботы потребляют относительно много электрической энергии, использование в них аккумуляторных батарей является предпочтительным.

Солевые батареи
Солевые батареи — это самые недорогие неперезаряжаемые источники питания.
Они могут быть использованы только в тех роботах, где отсутствуют мощные потребители электроэнергии — такие как двигатели, сильное освещение, работа по
радиоканалу. Они имеют малую емкость и нестабильное напряжение. Номинальное
напряжение для формата AA — 1,5 В.

Алкалиновые батареи
Алкалиновые батареи (также называются щелочными) имеют увеличенный по
сравнению с солевыми срок службы и хранения. Маркируются надписью
«Alkaline». Могут быть использованы в роботах с мощными потребителями электроэнергии. Номинальное напряжение для формата AA — 1,5 В.

Никель-металлогидридные аккумуляторы

Рис. 3.1. Никель-металлогидридные
аккумуляторы формата АА

Самым распространенным типом аккумуляторов
являются никель-металлогидридные (рис. 3.1), их
номинальное напряжение 1,2 В (формат AA) и
емкость от 900 до 3000 mAh. Для электропитания
робота на основе платы Arduino потребуется
шесть подобных аккумуляторов, соединенных
последовательно, что даст на выходе напряжение
7,2 В. Этого будет достаточно как для питания
платы Arduino (6–12 В), так и для моторной части
робота. Следует учитывать, что при последовательном соединении емкости батарей не складываются, поэтому для продления их жизни следует
применять совместно аккумуляторы с одинаковой
емкостью.

Литий-ионные и литий-полимерные аккумуляторы
Если в моменты пиковой нагрузки робот останавливается и перезагружается, значит, были выбраны элементы питания недостаточной мощности, которые не в состоянии выдавать требуемый роботу ток. При этом напряжение питания падает, что
и приводит к отключению электроники. Этого можно избежать, если параллельно

Электропитание

43

примененному источнику питания подключить другой, что увеличит максимальную отдаваемую мощность. Можно также запитать электронику робота от отдельного источника, не забыв в этом случае объединить отрицательные полюсы обоих
источников питания.
Еще одним выходом из подобной ситуации является использование более мощных
батарей — например, литий-ионных (рис. 3.2, слева). Для них стандартным номинальным напряжением является значение 3,7 В. Производятся литий-ионные
(Li-ion) аккумуляторы как в виде круглых батарей формата 18650, так и в виде пластин, широко применяемых в планшетах и сотовых телефонах. Емкость аккумуляторов формата 18650 составляет 1500–4000 mAh. Существует разновидность подобных аккумуляторов — литий-полимерные (Li-pol), отличие их от Li-ion в большем максимальном токе при сходных размерах, но меньшей емкости (рис. 3.2,
справа).

Рис. 3.2. Литиевые аккумуляторы: слева — литий-ионный; справа — литий-полимерный

Стабилизация электропитания
Важным моментом в электротехнике является наличие прочного контакта в цепи
источников питания — особенно это касается подвижных роботов, рывки и толчки
которых могут приводить к разрыву питающей цепи. Для того чтобы разрывов
питающей цепи не происходило, следует
использовать подпружиненные контакты и
специальные боксы с подобными контактами (рис. 3.3).
Электронные компоненты — такие как
контроллеры, датчики, приемопередающие
устройства — очень чувствительны к скачкам напряжения в цепи электропитания.
Аккумуляторы хотя и промаркированы номинальным напряжением, но реальное их
напряжение колеблется в зависимости от Рис. 3.3. Бокс для элементов питания

44

Глава 3

уровня заряда в широких пределах. Решением вопроса стабилизации электрического питания занимаются специальные приборы — стабилизаторы питания.

Стабилизация напряжения
Самым доступным понижающим стабилизатором напряжения питания является
микросхема КР142ЕН5А или ее аналог L7805CV. Схема подключения L7805CV
показана на рис. 3.4: на вход поступает нестабилизированное напряжение 7 В или
выше, а на выходе получаем стабильное постоянное напряжение 5 В.

а

б
Рис. 3.4. Стабилизация питания при помощи микросхемы L7805CV: а — схема соединений;
б — электрическая схема

45

Электропитание

Существуют подобные стабилизаторы и на другое напряжение, а также настраиваемые стабилизаторы. Платы Arduino UNO и Nano содержат как минимум один
стабилизатор напряжения на 5 и на 3,3 В.
Широкое применение получили портативные импульсные стабилизаторы. Они
имеют высокий коэффициент полезного действия, достигающий 95%.
В последнее время стали популярны импульсные понижающие стабилизаторы
китайского производства с настройкой выходного напряжения на основе микросхемы LM2596 (рис. 3.5).

Рис. 3.5. Понижающий импульсный стабилизатор на основе микросхемы LM2596

Существуют также повышающие импульсные стабилизаторы. Они могут, имея на
входе 3,7 В, преобразовать это напряжение в нужные для питания электроники
робота повышенные значения. Визуально они мало отличаются от понижающих
стабилизаторов напряжения, но работают на микросхеме XL6009 (рис. 3.6).
Хорошо зарекомендовал себя в работе повышающий импульсный стабилизатор со
входным напряжением от 2 В и стабилизированным выходным напряжением 5,1–
5,2 В (рис. 3.7). Такие стабилизаторы удобны, если требуется сэкономить на массе


Рис. 3.6. Повышающий импульсный стабилизатор
на микросхеме XL6009

Рис. 3.7. Повышающий импульсный
стабилизатор на заданное
напряжение 5,2 В

46

Глава 3

элементов питания и размерах создаваемого прибора, применяя низковольтную
батарею, — например, один литий-ионный аккумулятор на 3,7 В. Таким образом,
если для электропитания нашей схемы необходимо 5 В и небольшой ток (не более
0,5 А), можно получить довольно компактное устройство.

Стабилизация электрического тока
В некоторых случаях может потребоваться получить не стабильное напряжение, а
стабильный ток, — например, при подключении светодиодов. В этом случае используются стабилизаторы тока. Стабилизатор тока несложно построить на микросхеме LM317T (рис. 3.8). Значение стабилизируемого тока зависит от величины
сопротивления R1.

Рис. 3.8. Источник тока на базе LM317T

Измерение электрического тока,
напряжения и сопротивления
Для того чтобы точно знать параметры вашего источника электрического питания,
следует уметь производить измерения. Для этого применяются специальные приборы: амперметр измеряет величину тока, вольтметр — напряжение, омметр служит для измерения сопротивления электрической цепи и ее элементов. Существуют
приборы, которые могут измерять все указанные величины, — такие приборы называются мультиметрами (рис. 3.9).
Измерения осуществляются при помощи щупов, которыми шунтируется место
проведения замеров. Вращением ручки производится выбор режима измерения.
 Шкала, обозначенная значком Ω, служит для измерения сопротивления — измерения производятся в величинах, кратных обозначениям на шкале. Перед измерением сопротивления требуется обязательно обесточить измеряемый элемент,
иначе мультиметр может быть испорчен.
 Если у мультиметра есть выбор режима
, то возможно тестирование наличия
соединения между участками цепи, — это полезно, когда нужно установить
место разрыва.

Электропитание

47

Рис. 3.9. Мультиметр

 Шкала, обозначенная значком

, применяется для измерения напряжения постоянного тока. Может быть измерено напряжение питающей батареи, напряжение на различных элементах собранной схемы. Чтобы измерить напряжение для
элементов схемы, они должны быть запитаны. Так, если это двигатель, то на него
должно быть подано электрическое питание.

 Шкала, обозначенная значком

, предназначена для измерения напряжения
переменного тока. Такое напряжение имеется в сети переменного тока для
бытовых электроприборов.

 Шкала, обозначенная значком

, предназначена для измерения постоянного
тока. Ни в коем случае не измеряйте электрический ток в бытовых розетках —
это ОПАСНО! Электрический ток можно измерять в разрывах электрической
цепи, чтобы определить потребление вашей схемы.
Измерение тока элементов питания
Возможно кратковременное (не более секунды) измерение электрического тока химических элементов питания (литиевых батарей, алкалиновых батареек) с целью замера
их остаточного заряда, при этом производится выбор режима 10A, а активный щуп
(обычно красный) перекоммутируется из разъема VΩmA в разъем 10A. Ток нормально
заряженных алкалиновых батарей должен быть не менее 1 ампера, а литиевых аккумуляторов 18650 — не менее 2 ампер.

48

Глава 3

Защита от короткого замыкания по питанию
Возьмите солевую или алкалиновую батарейку формата ААА и на небольшое время (4–5 секунд) замкните проводом положительный и отрицательный контакт батареи. Батарея начнет нагреваться и через весьма непродолжительное время выйдет
из строя — это результат короткого замыкания по питанию. Мы выбрали для эксперимента именно батарейку формата ААА не случайно, дело в том, что она не
рассчитана на генерацию тока, величина которого приведет к серьезным последствиям, — например, к возгоранию или расплавлению изоляции провода. Если подобное сделать с Li-ion аккумулятором, то вполне возможен настоящий пожар.
У меня лично дело доходило до расплавления изоляции проводов и бокса, в котором находились аккумуляторы, при этом стальные контакты аккумуляторов аккумуляторного отсека накалялись докрасна, что может стать причиной пожара или
серьезных ожогов.
На рис. 3.10 приведен пример схемы короткого замыкания. Здесь в правильную
схему после выключателя добавлен проводник с надписью КЗ, который напрямую
соединяет между собой положительный и отрицательный контакты блока питания.
При этом ток потечет не через нагрузку, а по правилу наименьшего сопротивления
через проводник КЗ, что приведет к короткому замыканию со всеми описанными
последствиями. Как правило, короткое замыкание происходит вследствие ошибочного монтажа, плохой изоляции или случайного замыкания проводников питания.

Рис. 3.10. Короткое замыкание в электрической цепи

Как не допустить короткого замыкания?
1. Профилактика.
Мы уже научились пользоваться мультиметром. Вынем из робота батарею/
аккумулятор и включим мультиметр в режим тестирования наличия соединения
(режим
). Если у робота есть выключатель, установим его в положение
«Включено». Затем присоединим одну измерительную клемму к положительному контакту питания робота, а вторую — к отрицательному контакту (рис. 3.11).

Электропитание

49

Прибор может сразу показать наличие прямого соединения, а через несколько
секунд начать показывать высокое сопротивление, — это нормально и означает,
что заряжаются конденсаторы внутри робота. Если же и через несколько секунд
наличие прямого соединения не ушло, значит, в цепи вашего робота имеется короткое замыкание, и устанавливать в него элементы питания нельзя до устранения аварии.

Рис. 3.11. Схема замера мультиметром для профилактики короткого замыкания

Рис. 3.12. Установка предохранителя в цепь питания

50

Глава 3

2. Предохранители.
В цепь питания, например до выключателя питания, добавляем предохранитель
(рис. 3.12). Задача предохранителя — разорвать электрическую цепь, если ток
через предохранитель превысит установленный для конкретного предохранителя лимит. Существуют плавкие предохранители и восстанавливающиеся (многоразовые). Плавкий предохранитель при протекании через него высокого тока
плавится и размыкает цепь, восстанавливающийся предохранитель при протекании через него тока больше установленного значения нагревается и перестает
проводить ток.

Защита от неверной установки элементов
питания (переполюсовки)
На предшествующих рисунках в качестве нагрузки (потребителя) для блока питания мы приводили мотор постоянного тока — для него изменение направления
тока в цепи питания не страшно, вал мотора просто начнет вращаться в другую
сторону. Но есть потребители электричества, неправильное подключение которых
приведет к выходу их из строя, — к ним относятся электронные схемы, микроконтроллеры, умные датчики. Для защиты их от неправильного подключения можно установить диод или диод Шоттки (рис. 3.13). Диод проводит электрический ток
только в одну сторону, благодаря чему неправильное подключение аккумуляторов
приведет к тому, что устройство не включится, — на него просто не будет подано
напряжение.
На диоде при нормальном включении происходит небольшое падение напряжения:
для обычного диода примерно 0,5–0,6 вольта, для диода Шоттки — примерно
0,2 вольта. Из-за этого напряжение, подаваемое на вашу схему, будет немного
меньше того, что выходит от источника питания.

Рис. 3.13. Схема с защитой от переполюсовки питания

Электропитание

51

Зарядка аккумуляторов
на примере Li-ion и Li-pol
Разберемся, каким образом производится зарядка аккумуляторов, на примере
Li-ion-аккумулятора формата 18650 с номинальным напряжением 3,7 вольта (мы
планируем использовать для питания нашего робота именно их).
Существуют профессиональные устройства для зарядки аккумуляторов, которые
подключаются к сети ~220 вольт и заряжают, разряжают и тестируют аккумуляторы, но подобные устройства довольно дорогие и не всегда все их функции нам необходимы. Поэтому мы рассмотрим два самых простых примера.
На рис. 3.14 представлено зарядное устройство на основе модуля TP4056 для аккумуляторов формата 18650 с номинальным напряжением 3,7 В. Такой зарядкой, если
ее припаять к аккумуляторному боксу, можно заряжать аккумуляторы поочередно.
Модуль подключается к обычному зарядному устройству от смартфонов с разъемом micro-USB или USB Type-C (в зависимости от того, какая это версия модуля),
а к его контактам B+ и B– подключается заряжаемый аккумулятор. Если к контактам OUT+ и OUT– подключить нагрузку, модуль дополнительно защитит аккумулятор от глубокого разряда (отключит аккумулятор заранее). Когда идет заряд, на
модуле горит красный светодиод, после прекращении заряда загорается синий.

Рис. 3.14. Зарядное устройство на основе модуля TP4056 для одного аккумулятора

Следующий модуль заряда (рис. 3.15) появился недавно — его отличие от предыдущего в том, что он рассчитан на заряд двух последовательно соединенных аккумуляторов 18650. На практике этот модуль можно установить прямо на робота и
заряжать аккумуляторы, не вынимая их из бокса, что весьма удобно. Однако учтите, что аккумуляторы должны иметь одинаковую емкость, иначе один из них, что
с меньшей емкостью, быстро выйдет из строя.

52

Глава 3

Рис. 3.15. Зарядное утройство для пары последовательно соединенных аккумуляторов 18650

Выводы
Мы познакомились здесь с различными источниками электропитания, в том числе
наиболее эффективными никель-металлогидридными или литий-ионными аккумуляторами, которые и рекомендуется использовать для питания роботов. Рассмотрели наиболее распространенные схемы стабилизаторов питания — от них следует
запитывать контроллеры, датчики, приемопередающие устройства и другие чувствительные к питанию устройства, защиту от короткого замыкания и переполюсовки, а также простейшие устройства зарядки литий-ионных аккумуляторов.
В следующей главе мы обратимся к основам программирования контроллера
Arduino, на котором строится система принятия решений робота, — его «компьютера».

ГЛАВА 4
ОСНОВЫ ПРОГРАММИРОВАНИЯ
ARDUINO

Чтобы робот начал решать какую-либо задачу, собрать его недостаточно — потребуется создать и поместить в него специальную программу (план действий). Подобные программы называются компьютерными.

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

54

Глава 4

Алгоритм
Для визуального представления компьютерных программ используют их графическую запись, называемую блок-схемой алгоритма. Алгоритм, не
имеющий логических блоков и выполняемый строго
по пунктам от начала и до конца, называется линейным (рис. 4.1). Если алгоритм после достижения последнего пункта подразумевает бесконечное повторение, его называют циклическим (рис. 4.2). При наличии в алгоритме анализа некоторого значения и
принятия решения в результате такого анализа, алгоритм называется разветвляющимся (рис. 4.3). Алгоритмы легко анализировать и создавать — для
этого нужен только лист бумаги и карандаш. При
помощи алгоритмов можно описывать не только
компьютерные программы, но и любые планы —
например, планировать бизнес-процесс.

Начало

Ехать вперед 10
секунд

Повернуть направо
на 10 градусов

Конец
Рис. 4.1. Линейный алгоритм

Начало

Ехать вперед 10
секунд

Повернуть направо
на 10 градусов

Рис. 4.2. Циклический алгоритм

Рис. 4.3. Разветвляющийся алгоритм

Основы программирования Arduino

55

Алгоритмы можно записывать и в простой текстовой форме:
1. Ехать вперед 10 секунд.
2. Повернуть направо на 10 градусов.
А для разветвляющегося и циклического алгоритмов:
1. Получение информации о наличии препятствия с датчика.
2. Если препятствие есть, то перейти на пункт 5.
3. Ехать вперед 10 секунд.
4. Перейти на пункт 6.
5. Повернуть направо на 10 градусов.
6. Перейти на пункт 1.
Текстовая форма алгоритма больше похожа на компьютерную программу, но визуально воспринимается хуже, — в ней труднее найти логические ошибки, особенно
если переходов много.
Для того чтобы создать компьютерную программу, требуется ясно понимать, что
она должна делать, и главное, как она это будет делать. Если некоторые вопросы
вами еще не проработаны, а вы уже сели за написание серьезной программы, то,
скорее всего, она не будет работать так, как вы рассчитываете. Поэтому нужно идти
от простого к сложному: разбивать программы на маленькие простые блоки, добиваться их работоспособности и только затем собирать из них полную программу
для робота.

Среда разработки Arduino IDE
Перейдем к практике. В основу языка программирования, используемого в проектах Arduino, положен язык С++ — один из самых широко используемых языков
программирования, поддерживающий как работу с низкоуровневыми командами,
так и построение сложных объектов. Программирование контроллеров Arduino
удобно осуществлять в специальной среде Arduino IDE, поскольку в нее включен
основной функционал для работы с ними.

Установка Arduino IDE
Если у вас Windows 10, оболочку Arduino IDE можно установить из Microsoft Store
(магазина приложений Microsoft), в противном случае скачайте ее в Интернете по
адресу основного сайта проекта: www.arduino.cc/en/Main/Software. Программное
обеспечение Arduino IDE бесплатное, но если у вас есть желание помочь проекту,
это можно сделать, нажав кнопку CONTRIBUTE & DOWNLOAD, для простого
же скачивания нажмите кнопку JUST DOWNLOAD (рис. 4.4).
Выберите далее вариант Windows Installer и скачайте на компьютер программу инсталляции Arduino IDE — в настоящий момент это файл arduino-1.8.19-windows.exe
(но можно скачать и более новую версию — 2.0 RC). Файл надо запустить на выполнение

56

Глава 4

с административными полномочиями, принять условия лицензии GNU LESSER
GENERAL PUBLIC LICENSE и согласиться с предложенным вариантом установки.
На все предупреждения Windows в процессе установки следует отвечать утвердительно (продолжать установку). Когда установщик предложит установить драйверы
порта, также ответить утвердительно.

Рис. 4.4. Окно скачивания среды Arduino IDE

Начало работы с Arduino IDE
При запуске установленной Arduino IDE откроется окно (рис. 4.5), в котором уже
содержится заготовка программы. Она состоит из двух секций: setup и loop. Секция
setup содержит команды, выполняемые один раз при включении Arduino, — это
установка номеров портов ввода/вывода для управления моторами и установка
скорости обмена данными между Arduino и компьютером. Секция loop выполняется бесконечное число раз — до тех пор, пока мы не отключим питание. Фактически
она зациклена, алгоритмически это изображено на рис. 4.6.

Подключение контроллера Arduino к ПК
Теперь можно подключить контроллер Arduino к компьютеру. В зависимости от
того, какой контроллер Arduino используется, могут потребоваться разные кабели:
 для Arduino UNO R3 и Mega — это кабель с разъемом под USB-принтер с одной
стороны и стандартным USB-разъемом с другой (рис. 4.7);

Основы программирования Arduino

Рис. 4.5. Окно Arduino IDE с заготовкой программы

Рис. 4.6. Типовой алгоритм программы в Arduino IDE

57

58

Глава 4

 для контроллера Nano — кабель с разъемом mini-USB (рис. 4.8);
 для Arduino Micro — micro-USB;
 а для программирования контроллеров Arduino Mini и Pro Mini потребуется кон-

вертер USB-to-serial (рис. 4.9), т. к. у них отсутствует стандартный интерфейс
для подключения их к компьютеру.

Рис. 4.7. Кабель для Arduino UNO и Arduino Mega

Рис. 4.8. Кабель для Arduino Nano

Рис. 4.9. Конвертер USB-to-serial
с подключеной к нему платой Arduino Mini

Некоторым контроллерам нужен для работы нестандартный драйвер. Так, я встречал контроллеры Arduino, которые требуют установки отдельного драйвера, не
входящего в комплект Arduino IDE, — его название ch341ser.exe. Найти его в Интернете не трудно: https://www.wch.cn/downloads/CH341SER_EXE.html.
Физически подключив контроллер к ПК, следует установить связь между ним и
оболочкой Arduino IDE. Для этого нужно задать номер порта, к которому подключен контроллер (рис. 4.10). Если портов много и найти нужный сложно, рекомендуется запомнить все имеющиеся, а затем физически отсоединить Arduino от кабеля
и снова проанализировать список портов, — тот, который исчез, и есть нужный.
Подключайте снова контроллер и выбирайте появившийся порт — для этого надо
установить соответствующий ему флажок. Иногда для появления порта в списке
требуется некоторое время, за которое операционная система компьютера анализирует и проверяет подключенное устройство, так что подождите немного до завершения этого процесса
У пользователей Windows XP может возникнуть проблема, а именно — драйвер
порта контроллера Arduino не установится автоматически. Решение этой проблемы
описано на сайте Arduino по адресу: http://arduino.ru/Guide/Windows.
На следующем шаге выберем тип контроллера Arduino. На рис. 4.11 можно видеть,
что выбран Arduino Nano, если же у вас Arduino Uno, то выбирайте пункт
Arduino/Genuino Uno.

Основы программирования Arduino

Рис. 4.10. Выбор порта

Рис. 4.11. Выбор контроллера Arduino

59

60

Глава 4

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

Мигаем светодиодом
Загрузить написанную в Arduino IDE программу в контроллер, даже если она пустая, можно, нажав кнопку со стрелкой вправо
(см. рис. 4.5). Оболочка проверит
программу на наличие ошибок, а затем переведет ее в двоичный код данных и
команд выбранного микроконтроллера и запишет в Arduino.
Если после появления в нижней строке окна Arduino IDE слова Загрузка на плате
Arduino замигали светодиоды TX и RX, то загрузка программы в плату началась
успешно. Если после этого фраза Загрузка завершена не появилась, то, скорее
всего, неправильно выбран тип платы Arduino. Если же диоды TX и RX не замигали вообще, то проблема заключается в выборе порта платы Arduino.
Если же все пойдет штатно, то появится надпись Загрузка завершена и программа
автоматически начнет выполняться. Если загружена пустая программа, то ничего и
не произойдет, — пустой код будет бесконечно повторяться, пока к плате подключено электропитание.
Немного усложним программу, заставив Arduino мигать встроенным светодиодом
на порту 13 (листинг 4.1). Обычный диод — это электронный элемент, который
пропускает электрический ток только в одном направлении, а вот светодиод — это
диод, который начинает светиться при протекании по нему тока. На плате контроллера Arduino, как правило, уже установлен один светодиод — именно на порту
(ножке) 13.
Листинг 4.1. Программа мигания светодиодом
void setup() // Настройка.
{
// Переводим порт 13 в состояние вывода информации.
pinMode(13, OUTPUT);
}
// Основная программа.
void loop()
{
digitalWrite(13, 1);
// Включает светодиод на плате.
delay(1000);
// Ждет 1 сек.
digitalWrite(13, 0);
// Выключает светодиод.
delay(1000);
// Ждет 1 сек.
}

Основы программирования Arduino

61

Электронный архив
Напомню, что электронный архив с материалами к этой книге можно скачать с сервера
издательства «БХВ» по ссылке https://zip.bhv.ru/9785977517034.zip или со страницы
книги на сайте https://bhv.ru/. Архив содержит библиотеку для Arduino IDE с именем
mobrob3.zip — ее следует установить в среду Arduino IDE стандартным способом: Скетч |
Подключить библиотеку | Добавить .ZIP библиотеку. В результате у вас в Arduino
IDE на вкладке Примеры появится пункт mobrob3, содержащий листинги программ,
начиная с главы 5.

Цифровые порты могут получать и передавать информацию только в виде последовательности нулей и единиц. В приведенном примере как раз и передается на
порт 13 подобная последовательность — визуально это отслеживается по миганию
светодиода на плате контроллера. Когда светодиод горит, на порту высокое электрическое напряжение (5 В), а когда потушен — низкое (0 В).

Мониторинг работы программы
Научим контроллер общаться с компьютером во время выполнения программы
и запишем для этого в секции setup команду Serial.begin(9600), которая указывает, с какой скоростью происходит обмен информацией с ПК. Для обмена информацией требуется, чтобы приемник и передатчик осуществляли обмен на одинаковой

Рис. 4.12. Программа в окне Arduino IDE (слева) и результат ее работы в окне монитора порта (справа)

62

Глава 4

скорости, а т. к. контроллер Arduino достаточно медленный, то наиболее подходящей для обмена с ним является скорость 9600 бод. Обмен информацией на более
высоких скоростях также возможен, но задействует большее количество ресурсов
контроллера, при этом чаще происходят прерывания текущей программы, и основной код может выполняться медленнее.
Команда Serial.println() передает с контроллера на ПК значение, указанное
в скобках. В нашем примере это значение, представляющее собой время работы
в миллисекундах, возвращает (передает на компьютер) функция millis().
Программа и результат ее работы приведены на рис. 4.12. Для получения на экране
компьютера полученных результатов после запуска программы потребуется еще
открыть вкладку Инструменты | Монитор порта и, возможно, скорректировать
скорость порта (в программе это 9600 бод) — в окне Монитор порта скорость
задается в правом нижнем углу выбором из всплывающего списка.

Переменные
Важной составляющей синтаксиса языков программирования являются переменные. Переменные — это поименованная память, в которую можно вносить данные
и из которой можно брать данные. До начала работы с переменными их нужно
объявить. В C++ переменные можно объявлять практически в любых местах программы, но важно знать о зоне видимости переменных, — они видны лишь внутри
того блока, в котором объявлены.
Листинг 4.2. Объявление переменных. Зоны видимости. Сообщение об ошибке
// Переменные будут видны везде в программе.
int ex1;
float ex2;
void setup() // Настройка.
{
// Переменная будет видна только внутри секции setup.
int ex3;
ex3=8;
// Устанавливаем скорость порта связи Arduino - ПК.
Serial.begin(9600);
// значение ex1 изменится на 8*2.
ex1=ex3*2;
}
// Основная программа.
void loop()
{
// Следующая строка содержит ошибку:
// переменная ex3 не может быть видна за пределами секции setup.
ex3=ex1+5;
delay(ex1*100);
// Ждет 8*2*100 миллисекунд.

63

Основы программирования Arduino
// Передает на компьютер время работы в миллисекундах.
Serial.println(millis());
delay(1000);
// Ждет 1 сек.
}

Листинг 4.2 в демонстрационных целях содержит специально добавленную в него
ошибку объявления переменной, и при попытке его скомпилировать об этой ошибке будет выведено сообщение: 'ex3' was not declared in this scope.
Переменные могут отличаться по типу данных, для хранения которых созданы
(табл. 4.1).
Таблица 4.1. Типы данных
Обозначение
в программе

Название

Принимаемые значения

Размер
в памяти

boolean

Логический

True/false, 1/0

1 байт

char

Символьный

–128/+127

1 байт

byte

Короткое беззнаковое
целое

0–255

1 байт

int

Целое число

–32768/32767

2 байта

long

Длинное целое число

–2147483648/2147483647

float

Число с плавающей
точкой

38

–3,4028235·10 /3,4028235·10

4 байта
38

4 байта

При работе с переменными следует понимать, что они не являются идеальным хранилищем информации — так, например, целочисленные переменные могут переполняться. Это происходит в тех случаях, когда значение, которое следует записать
в переменную, больше максимально возможного для этого типа данных. Переменные с плавающей точкой подвержены другой проблеме — они округляют свои значения при сложении большого числа с малым. Например, сложение 1,1·1025 и 10
даст 1,1·1025, а число 10 просто потеряется. Также Arduino IDE не всегда корректно
работает с преобразованием типов данных.
Рассмотрим небольшую программу, осуществляющую получение данных от компьютера через порт ввода/вывода (листинг 4.3).
Листинг 4.3. Получение данных от компьютера через порт ввода/вывода
void setup() // Настройка.
{
// Устанавливаем скорость порта связи Arduino - ПК.
Serial.begin(9600);
}

64

Глава 4

// Основная программа.
void loop()
{
char char1;
// если поступили данные.
if (Serial.available() > 0)
{
// считываем символ.
char1 = Serial.read();
// отсылаем то, что получили, обратно на ПК.
Serial.println(char1);
}
}

Скомпилируйте и загрузите эту программу. Введите в верхней части окна Монитор
порта слово ПРИВЕТ! и нажмите кнопку Отправить. Программа отправит это слово
обратно на ПК посимвольно. Результат будет виден в нижней части окна:
П
Р
И
В
Е
Т
!

Условные операторы
Условные операторы мы уже использовали (см. листинг 4.3), а теперь рассмотрим
их подробно.

Оператор if ... else
Оператор If ... else графически, в виде блок-схемы алгоритма, представлен на
рис. 4.13, а в виде фрагмента кода — в листинге 4.4.

Рис. 4.13. Представление условного оператора

Основы программирования Arduino

65

Листинг 4.4. Применение оператора if ... else при управлении движением робота
if (distance < 10)
{ // Если дистанция до препятствия меньше 10 см.
Povorot(Right,10); // Поворот вправо на 10 градусов.
}
else // Иначе, если расстояние больше 10 см.
{
Vpered(); // Едем вперед 10 секунд.
delay(10000);
}

Как можно видеть, в этом фрагменте не раскрывается суть измерения расстояния
или того, как наш робот будет двигаться, — это скрыто во внешних функциях.
Создадим программу (листинг 4.5), которая изменяет частоту мигания встроенного
светодиода на порту 13 (рис. 4.14) в зависимости от посланной с компьютера
команды. Блок-схема алгоритма этой программы представлена на рис. 4.15.
Программа не завершается, пока есть электропитание. В ней присутствуют три
условных блока: один проверяет наличие данных на порту ввода/вывода, а последующие два сравнивают полученный символ с условиями, в зависимости от которых изменяют величину переменной time_pick, задающей время задержки при
мигании светодиода.

Рис. 4.14. Светодиоды на плате Arduino UNO

66

Рис. 4.15. Алгоритм изменения частоты мигания светодиода

Листинг 4.5. Программа управления миганием светодиодом с ПК
int time_pick; // Переменная для хранения времени свечения светодиода.
void setup() // Настройка.
{
// Устанавливаем скорость порта связи Arduino - ПК.
Serial.begin(9600);
// Переводим порт 13 в состояние вывода информации.
pinMode(13, OUTPUT);
time_pick=200; // Период свечения светодиода.
}

Глава 4

Основы программирования Arduino

67

// Основная программа.
void loop()
{
char char1;
// если поступили данные.
if (Serial.available() > 0)
{
// считываем символ.
char1 = Serial.read();
if(char1=='1') // Если нажата "1".
{time_pick=1000;} // Задержка мигания 1 сек.
else
{ //Если нажата "2". // Задержка мигания 0,5 сек.
if(char1=='2')
{
time_pick=500;}
// Если нажато что-то другое.
else {time_pick=100;}} // Задержка мигания 0,1 сек.
}
digitalWrite(13, 1);
// Включает светодиод на плате.
delay(time_pick);
// Ждет.
digitalWrite(13, 0);
// Выключает светодиод.
delay(time_pick);
// Ждет.
}

Оператор switch ... case
Следующий оператор: switch ... case — позволяет сделать выбор из набора определенных значений. Программа с его использованием выглядит намного проще,
и ее легче анализировать. Перестроим предыдущую программу, сохранив ее смысл
(листинг 4.6).
Листинг 4.6. Программа управления миганием светодиодом с ПК
с использованием оператора switch ... case
int time_pick; // Переменная для хранения времени свечения светодиода.
void setup() // Настройка.
{
// Устанавливаем скорость порта связи Arduino - ПК.
Serial.begin(9600);
// Переводим порт 13 в состояние вывода информации.
pinMode(13, OUTPUT);
time_pick=200; // Период свечения светодиода.
}

68

Глава 4

// Основная программа.
void loop()
{
char char1;
// если поступили данные.
if (Serial.available() > 0)
{
// считываем символ.
char1 = Serial.read();
switch (char1) {
case '1':
time_pick=1000;
break;
case '2':
time_pick=500;
break;
default:
time_pick=100;
}
}
digitalWrite(13, 1);
// Включает светодиод на плате.
delay(time_pick);
// Ждет.
digitalWrite(13, 0);
// Выключает светодиод.
delay(time_pick);
// Ждет.
}

Операторы циклов while и for
Очень удобными также являются операторы организации циклов. Рассмотрим два
таких оператора: while и for.
Цикл while продемонстрируем на примере программы из листинга 4.1, заменив
функцию ожидания delay() на оператор цикла по условию (листинг 4.7). Воспользуемся при этом функцией millis(), возвращающей количество миллисекунд от
старта программы. В результате работы этой программы светодиод включается
на 1 сек и выключается на 1 сек.
Листинг 4.7. Программа управления миганием светодиодом с ПК
с использованием оператора цикла while
void setup() // Настройка.
{
// Переводим порт 13 в состояние вывода информации.
pinMode(13, OUTPUT);
}
// Основная программа.
void loop()

69

Основы программирования Arduino
{
unsigned long time_1;
unsigned long time_2;
unsigned long time_3;
time_1 = millis();
// Начальное значение – количество миллисекунд
// от включения программы.
time_2=time_1+1000; // +1 секунду.
time_3=time_1+2000; // +2 секунды.
while(time_1<time_2)
{
digitalWrite(13, 1);
// Включает светодиод на плате.
time_1 = millis();
// Текущее время в time_1.
}
while(time_1<time_3)
{
digitalWrite(13, 0);
// Выключает светодиод на плате.
time_1 = millis();
// Текущее время в time_1.
}
}

Оператор цикла for хорошо подходит тогда,
когда требуется выполнить определенное количество повторений. При построении алгоритмов в виде блок-схем для него есть отдельная фигура (рис. 4.16).
В листинге 4.8 приведен пример программы,
управляющей миганием встроенного в плату
светодиода на порту 13, при этом светодиод
мигает 300 раз с увеличением времени свечения на 1 миллисекунду при каждом повторе,
а частота мигания уменьшается по мере приближения к концу цикла. Затем все начинается
сначала.
Рис. 4.16. Представление оператора for

Листинг 4.8. Пример использования оператора цикла for (на 300 повторений)
void setup() // Настройка.
{
// Переводим порт 13 в состояние вывода информации.
pinMode(13, OUTPUT);
}
// Основная программа.
void loop()

70

Глава 4

{
int i;
for(i=0;i<300;i++)
{
digitalWrite(13, 1);
delay(i);
digitalWrite(13, 0);
delay(i);
}

//
//
//
//

Включает светодиод на плате.
Ждет i миллисекунд.
Выключает светодиод.
Ждет i миллисекунд.

}

Функции
Функции представляют собой поименованные блоки команд, которые по их имени
можно вызывать из любого места программы. Функции нужно применять там, где
существует необходимость повторять одинаковый программный код много раз, или
если нагромождение команд в основной программе делает ее нечитаемой.
Существуют встроенные функции — они уже присутствуют в библиотеке функций
Arduino IDE, и функции, созданные в рамках текущей программы. Также разработаны библиотеки, которые можно подключать к текущей программе, и пользоваться имеющимися в них функциями. Подключаются библиотеки из пункта оболочки
Скетч | Подключить библиотеку. После подключения той или иной библиотеки
можно использовать содержащиеся в ней готовые программные решения, написанные для различных устройств и датчиков.
В листинге 4.9 приведен пример написания и использования простой функции —
она сравнивает два числа и возвращает наибольшее из сравниваемых. Для функций,
которые могут возвращать значения, при их описании в строке перед именем функции устанавливается тип возвращаемого значения, в нашем случае — это целое
число: int. Для возврата значения из функции служит ключевое слово return, за
которым следует возвращаемое значение.
При описании функций, не возвращающих значения, в строке перед их именем
должно стоять слово void.
Действия, производимые функцией, заключаются в фигурные скобки и называются
телом функции.
Листинг 4.9. Пример написания и использования функции
void setup() {
}
void loop() {
int x=10;
int y=25;

Основы программирования Arduino

71

// Вызов функции sravnenie().
int s=sravnenie(x,y);
}
// Определение функции sravnenie ().
// Указываем тип возвращаемого значения, имя функции и информацию
// о формальных аргументах.
int sravnenie(int a, int b)
// Далее идет тело функции.
{
if(a<b) return b;
else return a;
}

Элементы объектно-ориентированного
программирования
Язык C++, который составляет основу для программирования в проекте Arduino,
является объектно-ориентированным. Объекты — это сложные структуры, которые имеют оригинальное название и могут включать в себя как переменные, так и
функции. Фактически объекты созданы для удобного обращения к сложным структурам. Например, когда мы программно подключаем сервомотор (листинг 4.10), то
подключаем сначала библиотеку (#include <Servo.h>), а затем создаем объект
servomotor. Естественно, что при этом управляющий контакт сервомотора должен
быть физически подключен к порту Arduino, и на мотор подано электропитание
(рис. 4.17).
Листинг 4.10. Пример управления сервомотором
// Подключаем библиотеку для управления сервомоторами.
#include <Servo.h>
// Создаем объект сервомотор.
Servo servomotor;
void setup()
{
// Присоединяем сервомотор к пину 12 Arduino.
servomotor.attach(12);
}
void loop()
{
servomotor.write(10); // Поворачиваем вал сервомотора в положение 10°.
// Угол сервомотора увеличивается по часовой стрелке.
delay(500);// Задержка нужна, чтобы сервомотор успел повернуть вал.
servomotor.write(150);//Поворачиваем вал сервомотора в положение 150°.
delay(500);// Задержка на поворот.
}

72

Глава 4

Рис. 4.17. Подключение сервомотора к контроллеру Arduino UNO

Разделение программы
(внутренние библиотеки)
Несмотря на то что в каталоге с программой для Arduino IDE может находиться
только одна программа (файл с расширением ino), одноименная с каталогом, программу не обязательно «укладывать» в один файл. В рабочем каталоге с программой можно дополнительно создавать файлы с расширением h (рис. 4.18), а затем
подключать их к программе инструкцией #include "new_file.h", где new_file.h —
имя созданного дополнительного файла (оно может быть другим, но должно иметь
расширение h).
При этом содержимое подключенного файла при сборке программы включается
в тело основного файла программы.
Подключенные подобным образом файлы при открытии основной программы
автоматически открываются на редактирование в среде Arduino IDE в отдельных

Основы программирования Arduino

73

вкладках, что удобно для редактирования. В своей практике я создаю подобные
файлы для хранения библиотек функций. При этом для функций различной направленности создаю разные файлы. Таким образом, основной файл программы проекта
освобождается от лишней, затрудняющей понимание, информации.

Рис. 4.18. Пример разделения программы на два файла: основной — zerroSave.ino
и дополнительный — spider.h

Выводы
Начальное знакомство с языком программирования Arduino С++ завершено. Более
детальную информацию можно получить из тематической литературы и на сайтах:
arduino.cc — в разделе Documentation и arduino.ru — в разделе Программирование.
Далее в книге также будут рассмотрены типичные программы функционирования
роботов: управления двигателями постоянного тока, измерения расстояния датчиком-дальномером, взаимодействия с электронным компасом и др.
А в следующих двух главах мы познакомимся с конструкциями роботов и соберем
свою базовую модель.

ГЛАВА 5
ХОДОВАЯ ЧАСТЬ

Типы ходовых частей
Ходовая часть — это комплекс узлов и агрегатов, с помощью которых возможно
передвижение робота. Выбор ходовой части для робота, конечно, важный вопрос.
При этом можно исходить либо из поставленных перед роботом задач, либо из
имеющихся ресурсов, а если основная задача робота — это знакомство с робототехникой, то немаловажным критерием становится доступность деталей ходовой
части и простота ее изготовления. Рассмотрим некоторые виды ходовых частей.

Гусеницы
Гусеничная ходовая часть (рис. 5.1) весьма широко распространена в робототехнике благодаря простой реализации и высокой проходимости роботов, построенных на ее основе. Все, что будет описываться в практических главах, относится
и к ходовой части, реализованной на основе гусениц.

Рис. 5.1. Гусеничная ходовая часть

75

Ходовая часть

Если у вас есть под рукой гусеничная ходовая часть, например, от сломанной
радиоуправляемой модели, смело можете использовать ее для создания робота
по принципам управления, описываемым далее.

Колеса с дифференциалом
Колесная ходовая часть с дифференциалом часто встречается в радиоуправляемых
моделях машин. Как правило, в них задействованы два мотора постоянного тока: на
дифференциал задних колес и на рулевую тягу (рис. 5.2). Чтобы воспользоваться
материалом практических глав для создания робота на таком ходу, вам придется
существенно изменить функции, реализующие движения и повороты.

Рис. 5.2. Ходовая часть с дифференциалом на заднем мосту

Колеса на моторах
Колесные ходовые части с мотором на каждое колесо бывают двух видов: с двумя
моторными колесами и одним опорным поворотным роликом (рис. 5.3) и с четырьмя моторными колесами (рис. 5.4).

Рис. 5.3. Ходовая часть
с двумя ведущими колесами

Рис. 5.4. Ходовая часть
с четырьмя ведущими колесами

76

Глава 5

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

Летающие роботы
Раз уж мы начали говорить о ходовых частях, нельзя не упомянуть роботов, которые умеют летать или плавать. Когда вы изучите практические главы и поймете
принципы управления моторами и программную логику роботов, то реализация
более экзотических конструкций для полетов и плавания перестанет быть для вас
проблемой.
В настоящее время широкое распространение получили радиоуправляемые квадрокоптеры (рис. 5.5). Конечно, управление летающим роботом немного сложнее, да и
собрать подобного робота не просто.

Рис. 5.5. Квадрокоптер с установленной видеокамерой

Выбор двигателей
Итак, когда структура ходовой части выбрана, следует определить, какие двигатели
будет использовать наш робот. На выбор: коллекторные двигатели постоянного
тока с редуктором, сервопривод MG995 3600 постоянного вращения (скорость вращения задается сигналом на входе управления), шаговые двигатели. Их параметры
представлены в табл. 5.1.

77

Ходовая часть

Таблица 5.1. Сравнительные параметры различных типов двигателей
Двигатель
постоянного тока
с понижающим
редуктором

Сервопривод
MG995 3600

Шаговый
двигатель

Количество задействованных портов
микроконтроллера для управления

2–3

1

2–3

Скорость реакции на поступившую
команду, сек (чем больше, тем хуже)

~1/100

~1/50

~1/1000

Скорость вращения (с учетом
понижающего редуктора), об/мин

10–240

1–60

0,1–300

Стоимость (за единицу принята
стоимость двигателя постоянного
тока с редуктором), сравниваются
двигатели, равные по мощности

1

4

6

Минимальное количество портов
управления ходовой частью
для проекта колесного робота
с двумя ведущими колесами

4

2

4+1
дополнительный
контакт требуется
для отключения
всех моторов

Сравниваемые параметры

Что мы здесь видим?
 Коллекторные моторы с пластиковым редуктором — самые простые и самые

дешевые.
0

 Сервопривод MG995 360 — наиболее экономичный по ресурсам контроллера

Arduino. При его использовании хорошо регулируется скорость движения робота, если скорость колес нужно менять постоянно и держать в заданных пределах.
Скорость оборота колеса, вращаемого сервоприводом, зависит только от установленного на порту управления значения сигнала, и при увеличении сопротивления (когда, например, робот движется в гору) меняется слабо, в то время как
двигатель постоянного тока без обратной связи по скорости изменяет скорость
своего вращения в зависимости от нагрузки.
 Шаговый двигатель следует применять там, где нужна точность, и если это не-

обходимо, обратитесь к книге «Мобильные роботы на базе ESP32 в среде
Arduino IDE»1, там как раз используются шаговые моторы в качестве ходовой
части. Шаговым двигателем также можно держать постоянную скорость.
Все три вида двигателей применяются в ходовых частях колесных роботов, и выбор
зависит от требований, предъявляемых к роботу.
Однако мы только учимся, и сверхскоростной болид нам ни к чему, скорость реакции и скорость вращения не важны, а вот низкая стоимость имеет определяющее
значение на начальном этапе. Поэтому для дальнейшего рассмотрения выбираем
самое распространенное решение: коллекторный мотор с пластиковым редуктором.
1

См. https://bhv.ru/product/mobilnye-roboty-na-baze-esp32-v-srede-arduino-ide/.

78

Глава 5

Драйверы двигателей
Платы Arduino, кроме специализированных, не поддерживают возможностей
управления двигателями постоянного тока напрямую, и для этого нужно применять
специализированные микросхемы, называемые драйверами двигателей. Наиболее
распространенные из них приведены в табл. 5.2.
Таблица 5.2. Специализированные микросхемы драйверов двигателей (на платах)
TB6612FNG
(рис. 5.6)

MX1508
(рис. 5.7)

L298N
(рис. 5.8)

1,2

1,2

2

2

2

2

Максимальное напряжение питания, В

13,5

10

35

Минимальное напряжение питания, В

2,5

2,5

6

Есть

Есть

Есть

Характеристики
Максимальный ток на канал, А
Количество подключаемых коллектроных моторов

Встроенная диодная защита от паразитных токов

Все три платы драйверов подходят для управления направлением вращения и мощностью коллекторных двигателей постоянного тока. Они также могут применяться
и для управления шаговыми двигателями.

Рис. 5.6. Плата драйвера TB6612FNG

Драйверы TB6612 и L298N имеют отдельные входы управления мощностью моторов (PWMA–PWMB и ENA–ENB соответственно), но если на эти входы подать высокое логическое напряжение (5 вольт), то управление драйверами TB6612, L298N
и MX1508 будет осуществляться схоже по входам AIN1–AIN2 и BIN1–BIN2 — для
TB6612, а также IN1–IN2 и IN3–IN4 — для L298N и MX1508. Именно такие режи-

79

Ходовая часть

Рис. 5.7. Плата драйвера MX1508

Рис. 5.8. Плата драйвера L298N

мы работы мы и будем использовать, чтобы наша программа вела себя одинаково
и независимо от того, какой драйвер моторов вы решили использовать в своей
модели.
В табл. 5.3 приведены значения сигналов на входах и показана соответствующая
реакция двигателя.
Таблица 5.3. Значения сигналов на входах и соответствующая реакция двигателя
IN1 (AIN1)

IN2 (AIN2)

Мотор A (направление вращения условное)

0

0

Отключен

0

1

Вал вращается против часовой стрелки

1

0

Вал вращается по часовой стрелке

1

1

Отключен

80

Глава 5
Таблица 5.3 (окончание)

IN3 (BIN1)

IN4 (BIN2)

Мотор B (направление вращения условное)

0

0

Отключен

0

1

Вал вращается против часовой стрелки

1

0

Вал вращается по часовой стрелке

1

1

Отключен

Сборка макета
Для закрепления материала полезно собрать несколько макетов, которые продемонстрируют работу двигателей постоянного тока вместе с драйвером L298N,
можно также выполнить этот тест и на MX1508, а подключение TB6612FNG мы
рассмотрим позднее.

Управляем двигателем без Arduino
Возьмем драйвер L298N (или MX1508), двигатели постоянного тока, аккумуляторы
с боксом, провода, керамические конденсаторы 0,1 мкФ (маркируются числом
104) — они нужны для исключения электромагнитных наводок, которые возникают
при работе двигателей и могут стать причиной сбоев в работе электроники робота,
и паяльник. При пайке легко испортить поверхность стола, поэтому используйте
фанеру или постелите на место работы с паяльником пару листов бумаги.
Очистим и залудим паяльником двужильный провод (или пару одножильных),
затем залудим контакты двигателя и ножки конденсатора (рис. 5.9).

Рис. 5.9. Двигатель постоянного тока, керамический конденсатор и провода

81

Ходовая часть

Обмотаем ножки конденсатора вокруг залуженных кончиков проводов, как показано на рис. 5.10, — теперь ножки конденсатора хорошо держатся на проводе, и их
легко можно припаять (рис. 5.11).

Рис. 5.10. Ножки конденсатора обмотаны
вокруг оголенных концов провода

Рис. 5.11. Пайка конденсатора

Далее припаяем кончики проводов к залуженным контактам двигателя (рис. 5.12).
Если лужение контактов двигателя проходит плохо, аккуратно зачистите контакты
перочинным ножом или обработайте паяльной кислотой (после паяльной кислоты
нужно промыть контакты спиртом или водой).
Готовый к работе двигатель показан на рис. 5.13.

Рис. 5.12. Припаивание проводов к контактам двигателя

Рис. 5.13. Двигатель с припаянными проводами и конденсатором

82

Глава 5

Рис. 5.14. Схема тестирования двигателя

Качество пайки можно проверить, присоединив двигатель к аккумуляторам
(рис. 5.14), — если двигатель не работает, значит, пайка выполнена некачественно
(предварительно проверьте наличие напряжения на контактах аккумуляторного
бокса!).
Теперь, когда двигатели готовы, приступим к сборке схемы, изображенной на
рис. 5.15. Перемычки на контактах ENA и ENB драйвера не убираем!

Рис. 5.15. Схема управления двигателями без контроллера Arduino

Двигатели подключаются к драйверу через винтовые зажимы, также подключается
и аккумуляторный бокс, для контактов IN1–IN4 драйвера L298N лучше использовать провода с готовыми клеммами Dupont (см. главу 2), а для MX1508 требуется
пайка.
Подключая 5 вольт к различным контактам IN1–IN4, можно проследить, как изменяется вращение двигателей M1 и M2 (используйте данные табл. 5.4).
Подсказка
Контакты IN1 и IN2 отвечают за работу двигателя М1, а IN3 и IN4 — двигателя М2.

Широтно-импульсная модуляция
Проведем наглядный эксперимент: отключим контакт IN1 мотора М1, а контакт IN2
будем руками быстро подключать к напряжению 5 вольт и отключать от него, —
мотор станет вращаться с заметными рывками.

Ходовая часть

83

Если же делать эту операцию с высокой скоростью — такой, на которую способен
контроллер, то вал мотора будет вращаться без рывков, но мощность на валу окажется меньше, чем при постоянно включенном IN2. То есть передаваемая на вал
мощность зависит от частоты переподключения и длительности включения, и если
время включения мало, а время отключения относительно времени включения
велико, то и выдаваемая мощность будет мала, и наоборот. Этот подход к регулированию мощности вращения двигателей постоянного тока называется широтноимпульсной модуляцией.
Мы в наших проектах будем использовать шкалу мощности от 0 до 255, где число — это относительная длительность импульса включения мотора за период
(рис. 5.16). А чтобы иметь возможность изменять направление вращения, задействуем и отрицательную шкалу: –255...0...255, при этом изменяя вход драйвера, на
который подается сигнал (при положительном вращении — на IN1, при отрицательном — на IN2 для мотора М1 и IN3/IN4 — для мотора М2).

Рис. 5.16. Принцип работы ШИМ

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

84

Глава 5

Рис. 5.17. Схема управления двигателями для драйверов L298N и MX1508

На рис. 5.17 показана схема подключения драйверов L298N и MX1508, на рис. 5.18
и 5.19 — схема подключения TB6612FNG (она несколько сложнее).
Дело в том, что подключение драйвера TB6612FNG потребует немного больше соединений, т. к. нужно подать логическую единицу (5 вольт от Arduino) на контакты
VCC, PWMA, PWMB и STBY, в остальном же схема схожа с подключением L298N
и MX1508. Чтобы исключить на рисунках пересечение соединений и неверную их
трактовку, схема подключения драйвера TB6612FNG разбита на две части: подключение силовой части (рис. 5.18) и логической части (рис. 5.19).
Удобно при этом задействовать дополнительные платы, которые упрощают реализацию соединений, — например Arduino Sensor shield V5.0 (для Arduino Uno) или
NANO V3.0 Shield (для Arduino Nano).
Далее приведены примеры управления вращением двигателей при помощи программы, работающей на контроллерах Arduino UNO, Arduino Nano и Arduino Pro
Mini.

Ходовая часть

Рис. 5.18. Схема управления двигателями для драйвера TB6612FNG (силовая часть)

Рис. 5.19. Схема управления двигателями для драйвера TB6612FNG (логическая часть)

85

86

Глава 5

Подключаем библиотеку mobrob3.zip
и пишем тестовую программу
Специально для этого издания книги ее автор разработал библиотеку mobrob3.zip.
В ней, в частности, содержится модуль управления мощностью пары моторов
(см. далее).
Электронный архив
Электронный архив, включающий библиотеку mobrob3.zip, можно скачать с сервера издательства «БХВ» по ссылке https://zip.bhv.ru/9785977517034.zip или со страницы книги
на сайте https://bhv.ru/ (см. приложение).

Перед использованием библиотеку mobrob3.zip нужно добавить в Arduino IDE стандартным способом: Скетч | Подключить библиотеку | Добавить .ZIP библиотеку, и выбрать с диска файл mobrob3.zip. В результате все модули и примеры из этой
книги (по главам) станут доступны в Arduino IDE. Для управления моторами в библиотеку включен модуль mobrob3xmotor.h, в котором учтены некоторые особенности
управления моторами для генерации ШИМ, заключающиеся в том, что в качестве
левых моторов будущего робота можно использовать только контакты GPIO 2 и 3
(менять их местами можно), а для правого мотора — только 4 и 5 (их также можно
менять местами, чтобы добиться вращения в нужную сторону). Подобное ограничение связано с тем, что генерация ШИМ возможна только на контактах 3 и 5
GPIO.
Логика работы тестовой программы управления двигателями (листинг 5.1) следующая.
В бесконечном цикле:
1. Отключаем оба двигателя.
2. Ждем 1 секунду.
3. Подключаем двигатель 1 на полную мощность по часовой стрелке.
4. Ждем 1 секунду.
5. Отключаем двигатель 1.
6. Ждем 1 секунду.
7. Подключаем двигатель 1 на полную мощность против часовой стрелки.
8. Ждем 1 секунду.
9. Отключаем двигатель 1.
10. Возвращаемся на пункт 1.
Самостоятельно измените эту программу для управления двигателем 2.
Листинг 5.1. Тестовая программа управления двигателями
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3

Ходовая часть

87

// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include < mobrob3xmotor.h>
void setup()
{
// Вызываем функцию инициализации моторов.
motor_setup();
}
// Основная программа.
void loop()
{
motors_power(0, 0); // Отключены оба.
delay(1000);
motors_power(255, 0); // Включен мотор 1 +
delay(1000);
motors_power(0, 0); // Отключены оба.
delay(1000);
motors_power(-255, 0); // Включен мотор 1 delay(1000);
// Проделайте те же операции для двигателя 2 самостоятельно.
// ....
}

Добавляем регулирование на основе ШИМ
Рассмотренная и подключенная в предыдущем разделе библиотека mobrob3.zip уже
содержит модуль mobrob3xmotor.h, который может регулировать мощность моторов
при помощи ШИМ.
Напомню, что значения ШИМ в Arduino могут изменяться от 0 до 255. На практике
вал двигателя не сразу начнет вращаться — сначала (при малых значениях) двигатель станет гудеть и лишь по достижении определенного значения ШИМ начнет
медленно увеличивать обороты, что связано с недостатком мощности для компенсации сил трения.

Тестовая программа управления двигателями
с регуляцией на основе ШИМ
Логика работы этой тестовой программы (листинг 5.2) следующая.
В бесконечном цикле:
1. Отключаем оба двигателя.
2. Ждем 1 секунду.
3. Далее в цикле через 100 миллисекунд увеличиваем мощность от 70 до 255 для
обоих моторов.

88

Глава 5

4. Далее в цикле через 100 миллисекунд уменьшаем мощность от 255 до 70 для
обоих моторов.
5. Возвращаемся к пункту 1.
Итогом работы программы будет ускоряющееся вращение двигателей в одну сторону, а затем такое же торможение и остановка.
Самостоятельно измените программу так, чтобы моторы разгонялись в разные стороны.
Листинг 5.2. Тестовая программа управления двигателями
с регуляцией на основе ШИМ
// Зададим номера контактов управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
void setup()
{
// Вызываем функцию инициализации моторов.
motor_setup();
}
//=================================================================
// Основная программа.
void loop()
{
motors_power(0, 0);
delay(500);
for (int i = 70; i < 255; i++)
{
motors_power(i, i);
delay(100);
}
for (int i = 255; i > 70; i--)
{
motors_power(i, i);
delay(100);
}
}

Ходовая часть

89

Выводы
Итак, тип ходовой выбран — это колесная ходовая часть с двумя ведущими колесами. Выбран и используемый тип двигателей — это двигатель постоянного тока
с понижающим редуктором. Рассмотрен способ управления двигателями посредством специального драйвера. Определен порядок подключения и управления двигателями.
Для закрепления теоретических знаний собраны и опробованы на практике различные схемы управления двигателями постоянного тока. Следующий шаг — сборка
базовой модели робота.

ГЛАВА 6
СБОРКА БАЗОВОЙ МОДЕЛИ

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

Минимальный комплект
Итак, выбрана база с двумя моторами постоянного тока с редукторами (рис. 6.1),
корпус сделаем из тонкого упругого пластика или фанеры толщиной 3–4 мм. В качестве третьего колеса применим стандартное поворотное колесо в оправе общей
высотой порядка 35 мм, при этом диаметр используемых ходовых колес — 52 мм.
Если ваши колеса распространенного диаметра 65 мм, то поворотное колесо можно
взять повыше — 43 мм общей высоты.
Корпус можно вырезать из пластика или фанеры по приведенному здесь чертежу
(рис. 6.2) или распечатать на 3D-принтере, используя 3D-модель корпуса из электронного архива, сопровождающего книгу. В этом электронном архиве также приведены чертежи (модели) для печати колес. Шины для колес можно сделать из
пористой резины или распечатать из резиноподобного (TPU) пластика на 3D-принтере и приклеить к колесам. В электронном архиве есть и модели для печати элементов поворотного колеса под пару стандартных подшипников.
Заметим, что в качестве базовой модели также подойдет почти любой стандартный
корпус с моторами из тех, что можно приобрести в популярных интернет-магазинах, предлагающих запчасти для проектов на Arduino.
Набор комплектующих «Мобильные роботы на базе Arduino»
Издательство «БХВ» предлагает приобрести набор комплектующих, позволяющих построить роботы, описанные в этой книге. Информацию о том, как можно приобрести
такой набор, вы найдете по ссылке: https://bhv.ru/product/mobilnye-roboty-na-bazearduino-kniga/.

Сборка базовой модели

91

Рис. 6.1. Набор конструкционных деталей: винты и гайки (1); поворотное колесо (2);
корпус (3); моторы (4); пара колес (5)

Рис. 6.2. Чертеж корпуса робота с размерами для резки из фанеры

Поворотное колесо прикрутим винтами диаметром 3 мм. Чтобы винтовые крепления не ослаблялись со временем, я рекомендую нанести на винты в местах расположения гаек немного лака (подойдет цапонлак или лак для ногтей).

92

Глава 6

Двигатели
Перед установкой двигателей на ходовую часть следует припаять к контактным
площадкам двигателей провода (рис. 6.3). Лучше использовать для этого гибкие
многожильные провода разного цвета. Провода должны иметь достаточную длину,
чтобы не только достать до платы драйвера двигателей, но и быть аккуратно уложенными. Оставьте для них длину 12–15 см, а при монтаже к плате драйвера лишнее можно будет отрезать.
В последнее время получили распространение провода в силиконовой изоляции —
они весьма гибкие, и изоляция их не плавится при пайке, что особенно актуально,
если вы новичок в этой области.

Рис. 6.3. Двигатель с припаянными проводами

При пайке, если контакты двигателя сильно окислились и не поддаются лужению,
их следует немного почистить острым ножом — это ускорит процесс пайки. Но не
перестарайтесь — контакты можно сломать!
Настоятельно рекомендую при креплении проводов к двигателю использовать
именно пайку, но если она для вас недоступна, можно снять изоляцию с конца провода, конец без изоляции тщательно скрутить и продеть в ушко контакта двигателя,
после чего продетый конец обмотать вдоль той неизолированной части провода,
которая осталась до ушка контакта.
Для исключения электромагнитных наводок, которые возникают при работе двигателей и могут стать причиной сбоев в работе электроники робота, следует между
контактами каждого электрического двигателя впаять керамический конденсатор
(см. разд. «Управляем двигателем без Arduino» главы 5). Электромагнитные наводки могут влиять на радиоприем — например, если поднести радиоприемник к работающему двигателю, то ничего, кроме помех, слышно не будет. Конденсатор же
благодаря своим свойствам поглощает подобные высокочастотные скачки электрического напряжения. Для напряжения 5–12 В и используемых маломощных моторов достаточно конденсатора емкостью 100 нФ (рис. 6.4).

Рис. 6.4. Двигатель с припаянными проводами и конденсатором

Сборка базовой модели

93

Моторы приклеим к корпусу на клей «Момент-универсал» или «Момент-гель». Для
этого склеиваемые поверхности зачистим грубой наждачной бумагой, затем обезжирим, протерев изопропиловым спиртом или бензином-растворителем «Калоша».
После высыхания смажем обе склеиваемые поверхности клеем и обязательно подождем 15 минут, пока клей на обеих поверхностях не подсохнет. И только после
подсыхания клея следует хорошо прицелиться и плотно сжать склеиваемые поверхности. Если при этом мотор на корпус ляжет не точно, отдирание и повторное
сжатие не приведет к склеиванию — придется очистить поверхности от старого
клея и повторить процедуру склеивания сначала. Можно также приклеить моторы
на пористый двусторонний скотч.
Внимание!
Ни в коем случае не используйте супер-клей — он имеет высокую текучесть и может
частично затечь внутрь корпуса мотора и редуктора и вывести их из строя!

После завершения монтажа моторов платформа должна выглядеть так, как показано на рис. 6.5: поворотное колесо закреплено на винтах, моторы приклеены.
Теперь можно приступить к монтажу электроники и системы электропитания.

Рис. 6.5. Корпус с установленным снизу поворотным колесом и приклеенными моторами

Система электропитания
Элементы конструкции, которые применяются для монтажа электропитания робота, показаны на рис. 6.6. Система электропитания также исполняет функции защиты от короткого замыкания, фильтрации радиопомех, стабилизации напряжения,
зарядки аккумуляторов.
В качестве элементов питания в проекте выбран один либо пара литиевых аккумуляторов формата 18650 (см. рис. 6.6, позиции 8 и 9). Если планируется применять
автономное зарядное устройство, то аккумуляторы должны быть съемные и размещены в специальном аккумуляторном боксе (поз. 1 и 2). Бокс можно приклеить,

94

Глава 6

посадить на двусторонний скотч или прикрутить винтами с потайной головкой, используя имеющиеся в нем технологические отверстия (корпус робота для этого
придется просверлить по месту).

Рис. 6.6. Питание: бокс для аккумуляторов формата 18650 (1); бокс для аккумулятора (2);
выключатель (3); конденсатор электролитический (4); диод Шоттки (5); самовосстанавливающийся
предохранитель (6); керамические конденсаторы (7); аккумулятор формата 18650 (8);
аккумуляторы формата 18650 с контактами для пайки (9); встаиваемый модуль зарядки
для одного аккумулятора 18650 (10); встаиваемый модуль зарядки для двух последовательно
соединенных аккумуляторов 18650 (11); повышающий стабилизатор (12)

Если же вы собираетесь заряжать аккумуляторы непосредственно на роботе, потребуется портативный модуль заряда (поз. 10 или 11). Аккумуляторы можно приклеить клеем, качественным двусторонним скотчем или прикрепить нейлоновыми
стяжками (проделав сквозные отверстия под стяжки в корпусе). В этом случае
лучше использовать аккумуляторы, показанные на поз. 9, т. к. к ним приварены
контакты, которые легко паяются.
Аккумуляторы занимают много места, поэтому разместим их снизу — между поворотным колесом и моторами, а все остальное будем устанавливать сверху основной
«палубы».
Выключатель (поз. 3) будет отключать питание от положительного контакта робота. Электролитический конденсатор (поз. 4) предназначен для сглаживания питания
отдельных компонентов схемы (пока мы его не используем).

Сборка базовой модели

95

Диод Шоттки (поз. 5), установим на положительный контакт питания контроллера
Arduino — он позволит исключить подачу питания на моторы при программировании платы Arduino. Без диода Шоттки, если даже аккумуляторы отключены, питание с кабеля компьютера может попасть на моторы и привести к их нежелательному включению.
Восстанавливающийся предохранитель на 3–5 ампер (поз. 6) позволит избежать
неприятных последствий от короткого замыкания в цепи питания робота из-за
ошибок сборки.
Керамические конденсаторы (поз. 7) на редукторных моторах уменьшают электромагнитные помехи — в будущем это сослужит хорошую службу при управлении
роботом по радиоканалу (Bluetooth, Wi-Fi).
Повышающий стабилизатор, вариант которого показан на поз. 12, способен повышать входное напряжение до определенного значения. В нашем экземпляре стабилизатора величина выходного напряжения задается двумя перемычками, которые
нужно замкнуть или разомкнуть для получения требуемого напряжения на выходе
(схема изображена с обратной стороны платы).
На рис. 6.7 показаны разные варианты контроллеров Arduino и драйверов двигателей. Большой разницы в том, будете вы использовать UNO (поз. 1) или NANO,

Рис. 6.7. Варианты контроллеров и драйверов: плата Arduino UNO (1); плата Arduino Nano (2);
драйвер L298N (3); драйвер MX1508 (4); драйвер TB6612FNG (5)

96

Глава 6

(поз. 2) нет — на обеих платах установлен микроконтроллер ATmega328, но для
беспаечного монтажа UNO немного удобнее. Для этих плат можно применять платы расширения типа Sensor Shield (рис. 6.8), которые позволяют удобно соединять
плату Arduino с внешними устройствами при помощи проводов с разъемами
Dupont. Также NANO можно установить в макетную плату (см. рис. 1.29) и использовать ее функционал. В поз. 3, 4 и 5 на рис. 6.7 показаны варианты драйверов, которые будут точно совместимы с собираемым роботом.

Рис. 6.8. Платы расширения: плата Arduino Sensor Shield для UNO (1);
плата Arduino Sensor Shield для Nano (2)

Подбираем компоненты
Наш робот может питаться от одного или пары аккумуляторов, иметь или не иметь
встроенную зарядку, может использовать на выбор различные драйверы моторов,
но не все компоненты совместимы друг с другом. Для того чтобы разобраться
в совместимости и необходимости определенных компонентов, обратимся к данным табл. 6.1, предусматривающим четыре варианта комплектации нашего робота:
 вариант 1 — использование одного аккумулятора без встроенной зарядки;
 вариант 2 — использование одного аккумулятора со встроенной зарядкой;
 вариант 3 — использование пары аккумуляторов без встроенной зарядки;
 вариант 4 — использование пары аккумуляторов со встроенной зарядкой.

Выбор между TB6612FNG или MX1508 не очевиден — в классическом (приведенном в книге исполнении) TB6612FNG требует несколько дополнительных соединений, но он немного мощнее, чем MX1508. МХ1508 прост, если вы применяете
пайку, но не может быть установлен в макетную плату из-за нестандартного расположения выводов. Оба драйвера способны управлять моторами на низком напряжении от 2,5 вольт, и с 8,4 вольтами также работают без нареканий.

97

Сборка базовой модели

Таблица 6.1. Варианты комплектации электроники робота
Компонент

Вар. 1

Вар. 2

Вар. 3

Вар. 4

Рама

+

+

+

+

Поворотное колесо

+

+

+

+

Мотор-редукторы

2

2

2

2

Выключатель (тумблер)

+

+

+

+

Аккумулятор литий-ионый 18650

1

1

2

2

Бокс для 1×18650

1

–

–

–

Бокс для 2×18650

–

–

1

–

Встраиваемая плата заряда 1×18650

–

1

–

–

Встраиваемая плата заряда 2×18650

–

–

–

1

Плата Arduino UNO или Arduino NANO

1

1

1

1

Повышающий стабилизатор до 5 вольт или выше

1

1

–

–

Драйвер моторов с пониженным порогом
напряжения, подаваемого на моторы (от 2,5 вольт):
TB6612FNG или MX1508

+

+

+

+

Драйвер моторов с высоким порогом напряжения
питания (от 6 вольт): L298N

–

–

+

+

Для вариантов 1 и 2 предусмотрено питание от одного аккумулятора 18650 номинальным напряжением 3,7 вольт, чего достаточно, чтобы моторы робота работали,
но недостаточно для стабильной работы Arduino UNO/NANO. Поэтому здесь требуется повысить напряжение питания контроллера, чтобы иметь на нем стабильные
5 вольт, для чего используется плата повышающего стабилизатора. Если мы повышаем напряжение до 5 вольт, то подключаем напряжение со стабилизатора на вход
5V платы Arduino, если повышаем от 7 вольт и выше, то подаем это напряжение на
вход VIN. Отметим, что драйвер L298N не может управлять моторами от низкого
напряжения — ему требуется не менее 6 вольт питания.
Совет
Подавать напряжение от импульсного стабилизатора с выходом 5 вольт сразу на вход
5 вольт Arduino можно, если требования к точности аналоговых измерений, проводимых контроллером Arduino, не высоки или измерения вовсе не проводятся. Это связано с тем, что импульсный стабилизатор создает небольшие колебания выходного напряжения, что может отрицательно сказаться на аналоговых измерениях. Если вы нуждаетесь в точных аналоговых измерениях, повышайте входное напряжение уже до 8
вольт (перемычками на стабилизаторе) и используйте вход VIN Arduino — это приведет к тому, что входное напряжение будет поступать на внутренний стабилизатор, который его не только понизит, но и уберет колебания.

Варианты 1 и 3 не содержат плату заряда — для них необходим аккумуляторный
бокс, для вариантов 2 и 4 бокс не обязателен — у них предусмотрена плата заряда,
и аккумуляторы заряжаются прямо на роботе.
Более детально схемы комплектации роботов мы рассмотрим далее.

98

Глава 6

Схемы
На рис. 6.9–6.15 приведены схемы подключения для рассмотренных вариантов питания и драйверов моторов. На этих схемах не показано подключение зарядных
устройств — они должны быть подключены к контактам аккумуляторов/боксов,
как было показано в главе 3 на рис. 3.14 (для 1×18650) и на рис. 3.15 (для 2×18650).
Все приведенные в схемах аккумуляторы обладают номинальным напряжением 3,7
вольт.

Рис. 6.9. Схема электропитания по вариантам 1 и 2 с использованием драйвера TB6612FNG

Для определенности будем считать, что моторы робота расположены сзади, а поворотное колесо — спереди. Тогда мотор, подключенный к A01–A02 (TB6612FNG),
motor-A (MX1508) и мотор OUT1–OUT2 (L298N) управляют левым колесом, а мотор, подключенный к A03–A04 (TB6612FNG), motor-B (MX1508) и мотор OUT3–
OUT4 (L298N) — правым колесом.
Соблюдайте порядок подключения!
Важно соблюдать указанный на рис. 6.9–6.15 порядок подключения! Все программы
управления движением робота, приведенные в этой книге, ориентированы именно на
такие схемы. Если вы при монтаже поменяете местами двигатели и контакты управления, вам придется самостоятельно вносить изменения в управляющие программы
по всей книге, что может привести к путанице.

Сборка базовой модели

Рис. 6.10. Схема питания по вариантам 1 и 2 с использованием драйвера MX1508

Рис. 6.11. Схема электропитания по вариантам 3 и 4 с использованием драйвера TB6612FNG

99

100

Рис. 6.12. Схема электропитания по вариантам 1 и 2 с использованием драйвера MX1508

Рис. 6.13. Схема управления драйвером TB6612FNG по вариантам 1–4

Глава 6

Сборка базовой модели

101

Рис. 6.14. Схема управления драйвером MX1508 по вариантам 1–4

Рис. 6.15. Схема электропитания и управления по вариантам 3 и 4 с использованием драйвера L298N

102

Глава 6

В случае электропитания по вариантам 1 и 2 (от 3,7 вольт) в схему добавлен повышающий стабилизатор для гарантированного питания платы Arduino напряжением
не ниже 5 вольт. На рис. 6.9 и 6.10 приведены схемы повышения до 5 вольт. Если
повышать до 8 вольт, то подключать внешнее входное напряжение к контроллеру
Arduino нужно к контакту VIN, после чего установленный на плате контроллера
понижающий стабилизатор понизит напряжение до требуемых 5 вольт.
Также для всех схем установлен необязательный предохранитель по питанию —
его можно не ставить, как и диод Шоттки, — но рекомендуется во избежание печальных последствий от неверного подключения каких-либо контактов.
На рис. 6.15 показаны все соединения без пересечений для драйвера L298N. Контакты драйвера ENA и ENB должны быть замкнуты на 5V, а перемычки установлены по умолчанию, как показано, — проверьте, что они есть. В таком виде моторами
можно управлять, используя только 4 контакта: IN1–IN2 — для левого мотора и
IN3–IN4 — для правого мотора.

Проверка правильности подключения платы
драйвера и двигателей
Мы выбрали компоненты, установили их на плату (рекомендую для этого применять двусторонний скотч) и соединили по выбранной ранее схеме. Чтобы провода
не болтались, удобно прикрепить их к корпусу клеем «Момент» (согласно его инструкции) или двусторонним скотчем.
Далее, проверяем отсутствие короткого замыкания на входе питания драйвера
моторов (см. разд «Защита от короткого замыкания по питанию» главы 3).
Теперь протестируем работу моторов с помощью программы, приведенной в листинге 6.1, и, если нужно, поправим уже в программном коде установки моторов
(#define GPIO_1F ... GPIO_2B) — контакты GPIO управления моторами задаются
в начале программы.
Обратите внимание, что в этой программе задействован модуль mobrob3xmotor.h из
библиотеки mobrob3.zip (см. разд. «Подключаем библиотеку mobrob3.zip и пишем
тестовую программу» главы 5).
Загрузим программу. Вращаться вперед должно только левое колесо. Если левое
колесо вращается, но не вперед, а назад, поменяйте местами номера GPIO в начале
программы: 3 на 2 и 2 на 3.
Отметим, что GPIO_1F и GPIO_1B могут быть только 2–3 или 3–2, GPIO_2F и
только 4–5 или 5–4. Эти ограничения связаны с тем, что не все контакты
Arduino UNO/NANO могут по умолчанию генерировать ШИМ-сигнал, а мы пользуемся только двумя подобными: D3 и D5, (D2 и D4 не генерируют ШИМ-сигнал).
Если вы работаете на другой плате Arduino, например Mega, или вам нужны какието из занятых GPIO, вам придется самостоятельно внести правки в библиотечный
файл mra3xmotor.h. Проделайте подобное для правого колеса самостоятельно, добейтесь, чтобы положительное управление motors_power(150,0) вращало левое колесо
GPIO_2B —

Сборка базовой модели

103

вперед, а motors_power(-150,0) вращало левое колесо назад. Для правого колеса это
будут motors_power(0,150) и motors_power(0,-150) соответственно.
Листинг 6.1. Тестирование подключения моторов и драйвера к Arduino
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
void setup()
{
// Подключаем управление моторами.
motor_setup();
}
//=================================================================
// Основная программа.
void loop()
{
motors_power(150, 0);
delay(500);
}

Добейтесь правильного выполнения этого теста и запишите схему ваших контактов
(#define GPIO...) — она будет у вас оригинальная, и ее следует применять и
в дальнейшем, заменяя ту, что приведена в примерах книги.

Пример использования
платы Arduino Sensor Shield
Плата Sensor Shield может упростить сборку, если вы используете не пайку, а провода с контактами типа Dupont. Плата устанавливается сверху на Arduino UNO.
На плате Sensor Shield (рис. 6.16):
 джампер SEL платы оставляем замкнутым;
 под винтовой разъем GND зажимаем отрицательный провод от аккумуляторов;
 винтовой разъем VCC можно использовать для питания Arduino — зажимаем

провод от 5V повышающего стабилизатора или 5V драйвера L298N;
 если у вас используется вариант 3 или 4 с драйверами TB6612FNG или MX1508,

то VCC на этой плате не подключаем, а «плюс» питания подаем на контакт VIN
Arduino.

104

Глава 6

Рис. 6.16. Плата Arduino Sensor Shield v5.0 с пояснениями по закреплению проводов

Установка устройств обратной связи
Научим робота подавать сигналы в ответ на определенные события — например,
начало поворота, остановку и т. д. Для этого можно применить светодиод или зуммер, присоединенные к контактам D2–D13 или A0–A3 платы Arduino робота.
Светодиод (рис. 6.17) светится, если через него протекает ток
от анода к катоду. На анод светодиода следует подать положительное напряжение, а катод заземлить (на «минус»). При
этом светодиод будет нормально светиться лишь тогда, когда
к нему приложено напряжение, соответствующее номинальному (от 1,4 до 3 вольт — зависит от типа). Если же напряжение будет больше, он очень быстро выйдет из строя. Для того
чтобы светодиод светил долго, применяется токоограничивающий резистор.
Простейшая схема подключения светодиода показана на
рис. 6.18. Длинная ножка светодиода — анод, короткая — катод. Длинная ножка через резистор сопротивлением 200 Ом Рис. 6.17. Светодиод
будет подключена к контакту D6 платы Arduino, а короткая
ножка посажена на «землю» (GND). Обратите внимание, что если поменять ножки
светодиода местами, он не будет излучать свет.
Для подключения светодиода к проводам я использовал пайку: обрезал длинную
ножку и подпаял к ней резистор сопротивлением 200 Ом (рис. 6.19, а), затем подготовил провода с клеммами «мама» с одной стороны, а с другой стороны клеммы

105

Сборка базовой модели

Рис. 6.18. Схема подключения светодиода

а

б

в

г

Рис. 6.19. Пайка светодиода к проводам

отрезал, зачистил и залудил кончики (рис. 6.19, б), после чего подпаял провода
к светодиоду (рис. 6.19, в) и изолировал контакт с резистором, надев на него термотрубку — она сначала широкая, но после нагрева сильно стягивается, что очень
удобно (рис. 6.19, г).
Полученный светодиод с резистором удобно монтировать на плату Arduino Sensor
Shield v5.0: контакт с резистором надо подсоединить к ножке D6, а катод — к GND
(рис. 6.20).

106

Глава 6

Рис. 6.20. Подключение подготовленного светодиода к Arduino Sensor Shield v5.0

Для проверки загрузим в робота программу (листинг 6.2) — светодиод 10 раз мигает,
затем на 1 секунду гаснет.
Листинг 6.2. Мигаем светодиодом
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include < mobrob3xmotor.h>
// Создадим переменные для хранения номеров используемых
// пинов/портов Arduino.
int InDiod;
void setup()
{
// Подключаем управление моторами.
motor_setup();
// Присвоим переменным номера пинов Arduino.
InDiod = 6;
pinMode(InDiod, OUTPUT);
digitalWrite(InDiod, HIGH);
}
//=================================================================
// Основная программа.
void loop()

Сборка базовой модели

107

{
for (int i = 0; i < 10; i++)
{
digitalWrite(InDiod, HIGH);
delay(200);
digitalWrite(InDiod, LOW);
delay(200);
}
delay(1000);
}

Укладка проводов
Когда к роботу полностью подведено питание и управление, он скрывается под ворохом незакрепленных проводов. Болтающиеся провода, как правило, за чтонибудь цепляются, что приводит к их отсоединению или обрыву. Во избежание
таких проблем провода нужно объединять и укладывать. Это удобно делать при
помощи маленьких стяжек или изоленты. Можно также использовать термоклей —
для этого на корпус переносится капля термоклея, а затем в нее утапливаются провода. Если потом провода потребуется освободить, то термоклей нужно нагреть до
200 градусов, например феном.

Выводы
Робот собран (рис. 6.21). Теперь следует выполнить его контрольный осмотр, проверить, нет ли оторванных проводов, и можно будет приступить к созданию первой
программы, которая научит робота пользоваться моторами, совершать прямолинейное движение и повороты.

Рис. 6.21. Базовая модель робота в сборе

ГЛАВА 7
СХЕМА УПРАВЛЕНИЯ ДВИЖЕНИЕМ

Сборка робота закончена, теперь разберемся, как заставить его двигаться по некоторой программе.

Переменные и функции управления моторами
Напомню, что управление моторами осуществляется с помощью модуля
mobrob3xmotor.h из библиотеки mobrob3.zip (см. разд. «Подключаем библиотеку
mobrob3.zip и пишем тестовую программу» главы 5).

Подключение управления моторами
Успешное управление моторами возможно при следующей последовательности
описания его в программе:
1. Указать номера GPIO для моторов (#define GPIO_1F ... GPIO_2B) с учетом того,
что GPIO_1F и GPIO_1B могут быть только 2–3 или 3–2, а GPIO_2F и GPIO_2B —
только 4–5 или 5–4.
2. Подключить модуль mobrob3xmotor.h (#include <mobrob3xmotor.h>) — он содержит функции инициализации (установки параметров) и управления моторами.
3. Обозначить вызов в секции setup() функции инициализации моторов motor_
setup().

Управление мощностью
Управление мощностью моторов реализует функция motors_power(LM, RM), где
LM — левый мотор, RM — правый мотор (так далее принято для всех примеров). Значения обоих параметров функции можно задавать в пределах от –255 до +255:
при –255 мотор вращается с максимальной мощностью назад, при +255 — вперед,
при 0 — не вращается.

109

Схема управления движением

Движение в нужном направлении
Движение в нужном направлении осуществляется за счет сочетания различных
комбинаций параметров функции motors_power(LM, RM) (табл. 7.1). В таблице представлены граничные значения мощности, но можно использовать и половину мощности, и ее четверть, а также смешивать различные значения для пары моторов,
получая оригинальные повороты.
Таблица 7.1. Функции движений
Функция motors_power(LM, RM)

Назначение

motors_power(255,255)

Включает движение вперед

motors_power(0, 255)

Поворот налево с блокировкой левых колес

motors_power(255, 0)

Поворот направо с блокировкой правых колес

motors_power(-255,-255)

Включает движение назад

motors_power(0,0)

Остановка

motors_power(255,-255)

Поворот направо (на месте)

motors_power(-255,255)

Поворот налево (на месте)

Первая поездка
Для проверки работоспособности собранного робота нужно записать в него простую программу с определенной последовательностью движений. Созданные нами
функции только включают моторы с заданными параметрами, но не позволяют
управлять длительностью процесса. Поэтому нам следует позаботиться о том, чтобы после включения какой-либо функции — например, forward(), прошло некоторое время до включения следующей. За это время робот, отрабатывая поданную на
него команду, каким-то образом сместится. Естественно, что от времени, в течение
которого выполняется движение, зависит величина самого движения: если это поворот, то увеличение времени увеличит угол поворота, а если это движение вперед,
то увеличится пройденное расстояние.

Алгоритм
На рис. 7.1 представлен небольшой линейный (в нем нет условных операторов)
алгоритм тестовых движений робота. Согласно этому алгоритму робот начинает
двигаться, совершает повороты и проезды, а после выполнения всех команд начинает все сначала.
Если ваш робот совершает повороты и едет не так, как задумано в программе, не
спешите его разбирать — проверьте, какие колеса вращаются неправильно, и для
этих колес поменяйте местами номера контактов в программе.

110

Глава 7

Рис. 7.1. Линейный алгоритм тестовых движений робота

Программа
Напишем в соответствии с приведенным на рис. 7.1 алгоритмом небольшую тестовую программу (листинг 7.1), которая осуществляет в цикле несколько видов
движений. Ее цель — проверить, что все сделано правильно и колеса вращаются
в нужном направлении.
Листинг 7.1. Тестовая программа движений робота
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
void setup()

Схема управления движением

111

{
// Инициализируем моторы.
motor_setup();
}
//=================================================================
// Основная программа.
void loop()
{
// Пример движения робота задан жестко в программе
// и повторяется в цикле.
motors_power(255,255);
// едет вперед 1 секунду.
delay(1000);
motors_power(0,255);
// поворачивает налево 0,5 секунды.
delay(500);
motors_power(255,255);
// едет вперед 0,5 секунды.
delay(500);
motors_power(255,0);
// поворачивает направо 0,5 секунды.
delay(500);
motors_power(0,0);
delay(500);
motors_power(-255,-255); // движется назад 0,8 секунды.
delay(800);
}

Сигнал светодиодом
Модифицируем основную программу таким образом, чтобы при остановке робота
на нем загорался светодиод, подключенный к контроллеру Arduino (см. разд. «Установка устройств обратной связи» главы 6). Напомню только, что установлен он
был на управление от контакта 6 контроллера. Для этого нам всего лишь нужно
перевести контакт 6 в режим вывода и подавать на него 1 (HIGH) — чтобы светодиод загорелся, и 0 (LOW) — чтобы он погас (листинг 7.2).
Листинг 7.2. Робот движется и сигнализирует светодиодом
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Опишем переменную для хранения номера используемого
// пина/GPIO контроллера Arduino для подключения светодиода.
int InDiod;
void setup()

112

Глава 7

{
// Инициализируем моторы.
motor_setup();
// Присвоим переменным значение номера GPIO/пина светодиода.
InDiod = 6;
pinMode(InDiod, OUTPUT);
digitalWrite(InDiod, LOW);
}
//=================================================================
// Основная программа.
void loop()
{
// Пример движения робота задан жестко в программе
// и повторяется в цикле.
motors_power(255,255); // едет вперед 1 секунду.
delay(1000);
motors_power(0,255);
// поворачивает налево 0,5 секунды.
delay(500);
motors_power(255,255); // едет вперед 0,5 секунды.
delay(500);
motors_power(255,0);
// поворачивает направо 0,5 секунды.
delay(500);
motors_power(0,0);
// Включаем светодиод.
digitalWrite(InDiod, HIGH);
delay(500);
// Выключаем светодиод.
digitalWrite(InDiod, LOW);
motors_power(-255,-255); // движется назад 0,8 секунды.
delay(800);
}

Выводы
Робот научился исполнять команды, позволяющие ему двигаться и поворачивать, а
также сигнализировать при определенных ситуациях. Таким образом, нами создана
универсальная программа, на основании которой довольно просто сделать программу движений любой сложности, чередуя этапы включения определенных двигательных функций и времени, в течение которого они работают.
Однако недостатком нашего робота является то, что он никак не реагирует на изменение внешней обстановки — не может обходить препятствия и не умеет чтолибо анализировать. Причина этого в том, что анализировать ему пока нечего,
поскольку у него нет информации о внешнем мире. А чтобы у него появился материал для анализа, требуется оснастить его хотя бы одним датчиком, но прежде нам
следует научиться управлять роботом дистанционно, опираясь на материал главы 8.

ГЛАВА 8
ДИСТАНЦИОННОЕ УПРАВЛЕНИЕ
РОБОТОМ

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

работы.

Способы дистанционного управления
Удаленное управление подвижным роботом возможно по разным каналам. В главе 1
уже были упомянуты способы управления через инфракрасный канал, радиоканал
с использованием стандарта Bluetooth и радиоканал на основе стандарта Wi-Fi. Возможно также управление роботом посредством специализированной системы
управления, подобной используемой в качественных моделях летающих аппаратов
(рис. 8.1).
Нам необходимо выбрать решение недорогое, но в то же время довольно эффективное и не требующее сложного этапа проектирования и настройки. Дело в том,
что при использовании дешевых систем радиоуправления можно столкнуться
с проблемой, когда недорогие радиоуправляемые модели путаются в командах изза того, что воспринимают сигналы управления, направленные как к ним, так и
к другим подобным устройствам.
Выделим два приемлемых решения: управление по инфракрасному каналу (ИК/IR)
и управление по Bluetooth. Управление по IR-каналу самое простое, и реализовать
его можно, имея только IR-приемник (рис. 8.2), а в качестве пульта управления
воспользоваться любым IP-пультом — например, от телевизора. Но IR-прием работает только в пределах прямой видимости, что делает управление роботом не очень

114

Рис. 8.1. Профессиональный пульт дистанционного
управления роботом

Глава 8

Рис. 8.2. Инфракрасный приемник

эффективным. Управление роботом по Bluetooth лишено этого недостатка, но требует создания дополнительного устройства.

Управление роботом
по каналу инфракрасной связи
Рассмотрим для начала пример управления роботом при помощи IR-пульта. Готовый набор IR-компонентов для использования с Arduino-роботами представлен на
рис. 8.3. Подойдет нам также и любой инфракрасный пульт от телевизора или другой бытовой техники.
Для приема инфракрасного сигнала требуется IR-приемник. Он может быть смонтирован на плате (рис. 8.4, а) — в этом случае у нас будет возможность визуально
следить за приемом сигнала по миганию индикатора. Можно также использовать
IR-приемник без платы (рис. 8.4, б). Следует обратить внимание, что контакты платы IR-приемника и контакты IR-приемника без платы не совпадают.

Схема подключения
Пример подключения IR-приемника, уже установленного на плате, приведен на
рис. 8.5. Питание подано от платы Arduino, но можно подать его и от другого ис-

Дистанционное управление роботом

115

точника стабилизированного электропитания 5 В. На плате приемника (см. рис. 8.5,
справа) нижний контакт обозначен символом «–» — это «земля» (GND), верхний
контакт обозначен символом «S» — это сигнальный контакт (Data), средний контакт на плате не обозначен — это напряжение питания (Vcc = 5 В).

Рис. 8.3. IR-пульт (слева); плата IR-приемника (справа вверху);
соединительные провода (справа внизу)

Рис. 8.4. Плата IR-приемника (слева); IR-приемник без платы (справа)

116

Глава 8

Рис. 8.5. Подключение IR-приемника к Arduino UNO

Рекомендации по установке
Располагаться IR-приемник должен таким образом, чтобы быть в прямой видимости от пульта управления. На роботе это место не должно ничем перекрываться.
Общий вид робота в сборе, оснащенного IR-приемником, приведен на рис. 8.6.

Рис. 8.6. Робот, оснащенный IR-приемником

Дистанционное управление роботом

117

В случае использования IR-приемника без платы рекомендуется для уменьшения
помех и ложного срабатывания установить дополнительный фильтр питания
(рис. 8.7).

Рис. 8.7. Фильтр помех для IR-приемника

Установка библиотеки IRremote
Для работы с инфракрасными приемником требуется библиотека IRremote — ее
необходимо установить стандартным способом из среды Arduino IDE: Скетч |
Подключить библиотеку | Управлять библиотеками, затем в поле поиска вписать Irremote, выбрать из предложенного списка ту библиотеку, что указана на
рис. 8.8, и нажать кнопку Установка.

Получение кодов кнопок для используемого пульта
После установки библиотеки у вас в среде Arduino IDE появятся примеры для
управления роботом с помощью IR-пульта — вы можете поэкспериментировать
с ними самостоятельно.
Загрузите в робота программу из листинга 8.1. Проверьте, к какому контакту у вас
подключена сигнальная нога IR-приемника (по схеме, показанной на рис. 8.5, это
контакт 7). Теперь нужно запустить программу, открыть порт для просмотра кодов
кнопок пульта и дополнить полученными кодами из табл. 8.1, содержащей перечень функций движения робота и их назначение. Для пульта, показанного на
рис. 8.3, коды кнопок приведены в табл. 8.2.

118

Глава 8

Рис. 8.8. Установка библиотеки IRremote

Листинг 8.1. Проверка кодов кнопок пульта
// Подключение библиотеки IRremote.h.
#include <IRremote.h> // Подключаем глобально библиотеку из Arduino IDE.
// Порт Arduino для приемника.
int RECV_PIN = 7;
// Создаем объект IR-приемник.
IRrecv irrecv(RECV_PIN);
// Создаем структуру результата приема данных IR-канала.
decode_results results;
void setup()
{
// Устанавливаем скорость порта связи с ПК.
Serial.begin(115200);
// Запуск IR-приемника.
irrecv.enableIRIn();
}
void loop() {
// Если пришли данные.
if (irrecv.decode(&results)) {
// Послать полученные данные на ПК в шестнадцатеричном представлении.
Serial.println(results.value, HEX);
// Готов к приему.
irrecv.resume();
}
delay(100);
}

119

Дистанционное управление роботом

Таблица 8.1. Функции движения робота и их назначение
№
п/п

Функция

Назначение

1

motors_power(255,255)

Движение вперед

2

motors_power(0,255)

Поворот налево с блокировкой левых колес. Поворот
осуществляется при движении вперед, при переключении
от движения вперед к этому виду поворота не требуется
остановка

3

motors_power(255,0)

Поворот направо с блокировкой правых колес. Поворот
осуществляется при движении робота вперед, при
переключении от движения вперед к этому виду поворота
не требуется остановка

4

motors_power(-255,255)

Поворот налево на месте. Правые колеса вращаются
вперед, левые — назад

5

motors_power(255,-255)

Поворот направо на месте. Правые колеса вращаются
назад, левые — вперед

6

motors_power(-255,-255)

Движение назад

7

motors_power(0,-255)

Поворот налево с блокировкой левых колес. Поворот
осуществляется при движении задним ходом, при
переключении от движения назад к этому виду поворота
не требуется остановка

8

motors_power(-255,0)

Поворот направо с блокировкой правых колес. Поворот
осуществляется при движении задним ходом, при
переключении от движения назад к этому виду поворота
не требуется остановка

9

motors_power(0,0)

Остановка

Таблица 8.2. Соответствие кнопок пульта, изображенного на рис. 8.3,
действиям робота
Название
кнопки пульта

Код кнопки

Стрелка вверх

511DBB

Стрелка влево

Действие

Вызываемая функция

Двигаться вперед

motors_power(255,255)

52A3D41F

Поворот влево

motors_power(-255,255)

Стрелка вправо

20FE4DBB

Поворот вправо

motors_power(255,-255)

«Ok»

D7E84B1B

Стоп

motors_power(0,0)

Стрелка вниз

A3C8EDDB

Двигаться назад

motors_power(-255,-255)

«1»

C101E57B

Поворот влево
при движении вперед

motors_power(0,255)

«2»

97483BFB

Двигаться вперед

motors_power(255,255)

«3»

F0C41643

Поворот вправо
при движении вперед

motors_power(255,0)

«4»

9716BE3F

Поворот влево

motors_power(-255,255)

120

Глава 8
Таблица 8.2 (окончание)

Название
кнопки пульта

Код кнопки

«5»

Действие

Вызываемая функция

3D9AE3F7

Стоп

motors_power(0,0)

«6»

6182021B

Поворот вправо

motors_power(255,-255)

«7»

8C22657B

Поворот влево
при движении назад

motors_power(0,-255)

«8»

488F3CBB

Двигаться назад

motors_power(-255,-255)

«9»

449E79F

Поворот вправо
при движении вперед

motors_power(255,0)

«0»

1BC0157B

Стоп

motors_power(0,0)

«*»

32C6FDF7

Стоп

motors_power(0,0)

«#»

3EC3FC1B

Стоп

motors_power(0,0)

Программа
Программа для робота (листинг 8.2) предусматривает, что данные принимаются
с IR-датчика, коды, генерируемые пультом при нажатии той или иной кнопки,
взяты из табл. 8.2, принятые коды анализируются на совпадение, и при совпадении запускается та или иная функция, отвечающая за определенное движение
(см. табл. 8.1).
Следует учесть, что если кнопки не нажаты, то пульт не генерирует никакого сигнала, и эту ситуацию требуется обработать. Робот отслеживает время последнего
нажатия кнопки и останавливается через 0,3 секунды, если никакая кнопка не нажата. Если коды кнопок вашего пульта отличаются от приведенных в программе,
их следует скорректировать (они записаны после ключевых слов #define в виде:
РЕЖИМ код кнопки).
Листинг 8.2. Программа управления роботом с помощью IR-пульта
// Подключение библиотеки IRremote.h.
#include <IRremote.h> // Подключаем глобально библиотеку из Arduino IDE.
// Порт для IR-приемника.
int RECV_PIN = 7;
// Создание IR-приемника.
IRrecv irrecv(RECV_PIN);
// Переменная для хранения кодов кнопок, получаемых от IR-приемника.
decode_results results;
// Хранит время последнего нажатия кнопки.
unsigned long _time;
/////////////
// Опишем коды кнопок макроподстановками:
#define FORWARD
0x511DBB
#define LEFT
0x52A3D41F

Дистанционное управление роботом
#define RIGHT
0x20FE4DBB
#define STOP
0xD7E84B1B
#define BACKWARD
0xA3C8EDDB
#define FORWARDLEFT
0xC101E57B
#define FORWARD2
0x97483BFB
#define FORWARDRIGHT 0xF0C41643
#define LEFT2
0x9716BE3F
#define STOP2
0x3D9AE3F7
#define RIGHT2
0x6182021B
#define BACKWARDLEFT 0x8C22657B
#define BACKWARD2
0x488F3CBB
#define BACKWARDRIGHT 0x449E79F
#define STOP3
0x1BC0157B
#define STOP4
0x32C6FDF7
#define STOP5
0x3EC3FC1B
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Инициализация.
void setup()
{
// Инициализируем моторы.
motor_setup();
// Запуск IR-приемника.
irrecv.enableIRIn();
_time = millis();
}
// Основная программа.
void loop()
{
if (irrecv.decode(&results))
{
_time = millis();
switch (results.value) {
// Вперед.
case FORWARD:
motors_power(255,255);
break;
// Назад.
case BACKWARD:
motors_power(-255,-255);
break;

121

122

Глава 8
// Влево.
case LEFT:
motors_power(-255,255);
break;
// Вправо.
case RIGHT:
motors_power(255,-255);
break;
// Вперед.
case FORWARD2:
motors_power(255,255);
break;
// Назад.
case BACKWARD2:
motors_power(-255,-255);
break;
// Влево.
case LEFT2:
motors_power(-255,255);
break;
// Вправо.
case RIGHT2:
motors_power(255,-255);
break;
// Прямо и влево.
case FORWARDLEFT:
motors_power(0,255);
break;
// Прямо и вправо.
case FORWARDRIGHT:
motors_power(255,0);
break;
// Назад и влево.
case BACKWARDLEFT:
motors_power(0,-255);
break;
// Назад и вправо.
case BACKWARDRIGHT:
motors_power(-255,0);
break;
// Стоп.
case STOP:
motors_power(0,0);
break;
case STOP2:
motors_power(0,0);
break;

Дистанционное управление роботом

123

case STOP3:
motors_power(0,0);
break;
case STOP4:
motors_power(0,0);
break;
case STOP5:
motors_power(0,0);
break;
}
irrecv.resume();
}
// Если никакая клавиша не нажата более 0,3 сек., то остановка.
if ((millis() - _time) > 300) motors_power(0,0);
}//=====================================================================

Управление роботом по каналу Bluetooth
Для связи с роботом по каналу Bluetooth потребуется установить на него контроллер Bluetooth — тогда управлять роботом можно будет со смартфона на операционной системе Android. В магазине приложений для Android-устройств Google Play
Маркет есть достаточное количество бесплатных приложений, реализующих функции управления колесными роботами (рис. 8.9).

Рис. 8.9. Программы управления роботами на Google Play Маркет (для смартфонов на Android)

124

Глава 8

Подбор элементной базы
В качестве Bluetooth-контроллера робота можно применить недорогие модули
HC-05/HC-06 (рис. 8.10) или SPP-C (рис. 8.11). Питание 5 В подается на контакты
VCC (+5) и GND. Для работы на прием и передачу используются контакты RXD
и TXD соответственно. На плате Arduino робота потребуется задействовать для работы с этим Bluetooth-контроллером два свободных контакта GPIO, на которых
возможно открыть для обмена порт Serial.

Рис. 8.10. Модуль HC-05/HC-06 приемопередачи по Bluetooth

Рис. 8.11. Модуль SPP-C приемопередачи по Bluetooth.
Назначение контактов то же, что и на рис. 8.10

Если вы хотите присвоить контроллеру Bluetooth оригинальное имя, отличное от
имени по умолчанию, понадобится выполнить один раз определенные команды.
При этом для модуля HC-05 (имя по умолчанию HC-05) может потребоваться еще

Дистанционное управление роботом

125

один контакт GPIO (для разрешения записи). После смены имени этот контакт
можно будет освободить (см. подробности в разд. «Смена имени контроллера
Bluetooth и робота» далее).
Вариант Bluetooth-модуля SPP-C удобен тем, что не требует применения пайки для
изменения имени и других настроек. После подключения по умолчанию ему присваивается имя BT04-A. Подключается он аналогично HC-05, но есть небольшое
отличие в синтаксисе написания программ: при использовании AT-команд между
командой и параметром не ставится знак «=». Например, команда присвоения имени модулю для HC-05 выглядит так:
AT+NAME=ROBOPES

а для SPP-C и HC-06 так:
AT+NAMEROBOPES

По этой команде модулю будет присвоено имя ROBOPES.
За этим небольшим исключением все написанное далее одинаково для рассмотренных модулей. Еще стоит отметить, что HC-06 визуально не отличается от HC-05,
но в HC-06 отсутствует ряд режимов работы, которые в этой книге не рассматриваются.

Подключение к Arduino
Обычно Bluetooth-контроллер подключается на стандартные входы последовательного порта D0 и D1 — на плате Arduino они обозначены как RX и TX. Но эти же
входы используются при программировании контроллера Arduino, поэтому каждый
раз при проведении процедуры программирования вам придется отсоединять питание Bluetooth-контроллера, иначе он будет мешать обмену информацией между
компьютером и Arduino. На практике можно установить маленький переключатель,
разрывающий питание Bluetooth-контроллера, чтобы он не «работал» и не мешал
при подключении контроллера Arduino к компьютеру для программирования.
На Arduino Mega есть четыре порта Serial, и можно не ломать себе голову над тем,
как подключить Bluetooth-контроллер, а подключить его на 1 (19 и 18), 2 (17–17)
или 3-й (15–14) порт.
Для UNO и Nano можно также задействовать библиотеку SoftwareSerial.h (специальную библиотеку эмуляции последовательного порта) — в этом случае подключать Bluetooth-контроллер можно на контакты GPIO, отличные от D0 и D1, но,
к сожалению, ее применение дает сбои при работе с гироприбором MPU6050, который мы будем активно использовать в следующих главах. Это связано с тем, что
библиотека SoftwareSerial.h для эмуляции порта опирается не на аппаратные, а на
программные ресурсы контроллера Arduino, что приводит к тому, что обмен
информацией между Arduino и гироприбором MPU6050 часто прерывается и не
завершается корректно.
Подключение должно быть перекрестным: RXD Bluetooth-контроллера подключается к TX Arduino, а TXD Bluetooth-контроллера — к RX Arduino. Контакты RXD и

126

Глава 8

RX читают данные, а TXD и TX их передают — соответственно в паре соединенных контактов один должен быть передающим, а другой — читающим. Это нужно
будет учесть при соединении последовательного порта. Порядок соединения приведен на рис. 8.12.

Рис. 8.12. Подключение контроллера Bluetooth HC-05 к Arduino UNO с контактом
для модификации имени

Подключение контакта 34 контроллера Bluetooth к порту A0 актуально, только если
требуется сменить имя модуля HC-5 — после смены имени контакты 34 контроллера Bluetooth и A0 Arduino можно будет освободить (см. далее). Для смены имени
на HC-06 или SPP-C задействовать контакт 34 не потребуется.
Теперь, если включить робота и выполнить на смартфоне поиск Bluetoothустройств, в нем появится новое Bluetooth-устройство — оно будет называться так,
как всю партию прошили на фабрике, — например, HC-05.

Смена имени контроллера Bluetooth и робота
HC-05 или HC-06 — не очень оригинальные названия, но они прошиты внутри модулей, и их можно изменить. В отличие от SPP-C и HC-6, перепрошивка HC-05
возможна только при наличии логической единицы на его контакте 34. Для подачи такой логической единицы потребуется присоединить к контакту 34 HC-05

Дистанционное управление роботом

127

(см. рис. 8.12) свободный порт контролера Arduino — пусть это будет порт A0. Согласно инструкции на Arduino UNO порты A0–A7 являются аналоговыми приемниками, но A0–A5 могут также работать и в двоичном режиме как для чтения, так и
для вывода информации. Контакт 34 расположен довольно близко к контакту 33 —
паяйте осторожно! После изменения имени контроллера Bluetooth контакт 34 можно будет освободить. Изменение имени контроллера Bluetooth соответственно
меняет и имя робота.
Программа переименования робота для описанных настроек (используемых портов) приведена в листинге 8.3. Новое имя присваивается роботу в строке BTSerial.
println("AT+NAME=MaxiBot") — здесь это имя MaxiBot. Придумайте и задайте имя
своему роботу латинскими буквами без пробелов.
Проверьте результат прошивки, проведя поиск новых устройств Bluetooth на
смартфоне.
Листинг 8.3. Переименование робота и проверка работы Bluetooth
// Переменная для приема данных по Bluetooth.
char bt_input;
// Настройка.
void setup()
{
// Устанавливаем скорость передачи данных по Bluetooth.
Serial.begin(9600);
// Переключаем A0 в двоичный режим работы, на передачу.
pinMode(14, OUTPUT);// Только для HC-05.
// Переводим HC-05 в режим AT-команд.
digitalWrite(14, 1); // Только для HC-05.
// Присваиваем HC-05 новое имя – MaxiBot.
Serial.println("AT+NAME=MaxiBot");// Для НС-05.
// Для модуля из набора HC-06 и SPP-C.
// Serial.println("AT+NAMEMaxiBot");
// Переводим HC-05 в нормальный режим работы.
digitalWrite(14, 0); // Только для HC-05.
}
void loop()
{
}

Настройка смартфона
Для управления роботом по каналу Bluetooth вам потребуется смартфон на операционной системе Android и программа удаленного управления. Удачным выбором
может стать программа Arduino Bluetooth RC Car. Она хорошо документирована
и неплохо зарекомендовала себя в тестах. Установите ее на смартфон с Google Play
Маркет. Интерфейс программы приведен на рис. 8.13.

128

Рис. 8.13. Главное окно программы Arduino Bluetooth RC Car

Рис. 8.14. Выбор Bluetooth-устройства

Рис. 8.15. Соединение установлено

Глава 8

Дистанционное управление роботом

129

Чтобы подключиться к роботу, следует нажать кнопку в виде шестеренки, активировать Bluetooth, после чего выбрать пункт Connect. Если робот уже прописан
в устройствах смартфона, нужно будет выбрать его из меню (рис. 8.14), в противном случае произвести новое сканирование и подключить робота. Пароль по умолчанию у HC-05: 1234 — это может потребоваться!
Когда робот соединится со смартфоном, интерфейс программы изменится: серая
лампочка слева вверху станет зеленой, а в верхнем правом углу появится имя робота, к которому подсоединен смартфон (рис. 8.15).

Устранение радиопомех
Когда речь идет о работе по радиоканалу, возникают проблемы, связанные с электромагнитной совместимостью. В главах 5 и 6 уже рассматривался вопрос защиты
роботов от помех, создаваемых собственными электромоторами. Точно так же установка Bluetooth-модуля рядом с сервомотором головы, создающим радиопомехи,
может приводить к частым обрывам связи. Лучше всего (но не обязательно), если
Bluetooth-модуль будет расположен на небольшой антенне и подальше от двигателей. При этом если на все моторы будут установлены рекомендованные в главах 5
и 6 защитные конденсаторы, то радиус связи вашего робота значительно увеличится и обрывы связи сократятся.

Программа
Программа управления роботом по каналу Bluetooth весьма проста: робот постоянно проверяет на последовательном порту Bluetooth-контроллера наличие байта команды. Если команда есть — изменяет свои действия в зависимости от того, какой
байт пришел. Если никакие кнопки на смартфоне не нажимать, он все равно отправляет команду, а именно — символ S, которую наш робот интерпретирует как
остановку.
Приведенная в листинге 8.4 программа позволяет роботу поворачивать на полном
ходу, используя принцип блокировки колес со стороны поворота. Если же робот
стоит, то он поворачивает всеми колесами, — при этом правые и левые вращаются
в разные стороны. Робот может поворачивать и при езде задним ходом. Программа
Arduino Bluetooth RC Car также дает возможность управлять роботом посредством
изменения ориентации смартфона в пространстве.
Кроме заявленных функций, программа Arduino Bluetooth RC Car позволяет, например, включать и отключать фары, если включение фар предусмотрено конструкцией робота. Подробнее прочитать о том, какие символы-команды посылает
смартфон роботу, можно на странице помощи в программе управления на смартфоне.
Для того чтобы можно было управлять мощностью двигателей, в программу введена переменная Power, в которой задается мощность моторов.

130
Листинг 8.4. Управление роботом по Bluetooth-каналу
// Переменная для приема данных по Bluetooth.
char bt_input;
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Инициализация.
void setup()
{
// Устанавливаем скорость передачи данных по Bluetooth.
Serial.begin(9600);
// Переключаем A0 в двоичный режим работы, на передачу.
pinMode(14, OUTPUT);//Только для HC-05
// Переводим HC-05 в режим AT-команд.
digitalWrite(14, 1); // Только для HC-05.
// Присваиваем HC-05 новое имя - MaxiBot
Serial.println("AT+NAME=MaxiBot");// Для НС-05
// Для модуля из набора HC-06 и SPP-C.
// Serial.println("AT+NAMEMaxiBot");
// Устанавливаем скорость передачи данных по кабелю.
// Переводим HC-05 в нормальный режим работы.
digitalWrite(14, 0); // Только для HC-05
// Инициализируем моторы.
motor_setup();
}
const unsigned long move_time = 300;
unsigned long _time = 0;
int Power=100;
void loop()
{
// Если пришли данные по Bluetooth.
if (Serial.available())
{
// Записываем, что пришло по Bluetooth, в переменную bt_input.
bt_input = (char)Serial.read();
_time = millis();
switch (bt_input) {
// Вперед.
case 'F':
motors_power(Power, Power);
break;

Глава 8

Дистанционное управление роботом
// Назад.
case 'B':
motors_power(-Power, -Power);
break;
// Влево.
case 'L':
motors_power(-Power, Power);
break;
// Вправо.
case 'R':
motors_power(Power, -Power);
break;
// Прямо и влево.
case 'G':
motors_power(0, Power);
break;
// Прямо и вправо.
case 'I':
motors_power(Power, 0);
break;
// Назад и влево.
case 'H':
motors_power(0, -Power);
break;
// Назад и вправо.
case 'J':
motors_power(-Power, 0);
break;
// Стоп.
case 'S':
motors_power(0, 0);
break;
// Скорость 0%.
case '0':
Power = 100;
break;
// Скорость 10%.
case '1':
Power = 115;
break;
// Скорость 20%.
case '2':
Power = 130;
break;
// Скорость 30%.
case '3':
Power = 145;
break;

131

132

Глава 8
// Скорость 40%.
case '4':
Power = 165;
break;
// Скорость 50%.
case '5':
Power = 180;
break;
// Скорость 60%.
case '6':
Power = 195;
break;
// Скорость 70%.
case '7':
Power = 210;
break;
// Скорость 80%.
Power = 225;
break;
// Скорость 90%.
case '9':
Power = 250;
break;
// Скорость 100%.
case 'q':
Power = 255;
break;
case 'X':
//
break;
case 'x':
//
break;
case 'D':
motors_power(0, 0);
break;
}
_time = millis();

}
if ((millis() - _time) > move_time)

motors_power(0, 0);

}

Основные проблемы, которые могут возникнуть при подключении контроллера
Bluetooth, связаны c электромагнитной несовместимостью и невнимательностью.
Если контроллер Bluetooth не изменяет свое имя с первого раза, следует сделать
повторную попытку.
Принцип управления роботом по каналу Bluetooth теперь должен быть вам понятен. Здесь не рассмотрен вопрос обратной передачи информации от робота и связи

Дистанционное управление роботом

133

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

Управляем роботом по Wi-Fi (ESP-01)
В последнее время хорошо зарекомендовали себя микроконтроллеры марки ESP —
их существует много разновидностей, они применяются в различных областях,
в том числе и в Arduino-проектах. Рассмотрим модуль ESP-01 — это модуль на
основе контроллера ESP8266, но в форм-факторе приемопередатчика (рис. 8.16).
ESP-01 имеет 8 контактов, но мы будем использовать только 5:
 VCC — 3,3 вольта с платы Arduino;
 GND — на GND платы;
 TX — на контакт 10 платы;
 RX — на контакт 11 платы;
 CH_PD (enable) — на VCC ESP-01.

Рис. 8.16. Модуль ESP-01 с обозначениями контактов

Соединяться с роботом мы станем так же, как и в предыдущем примере, — по порту Serial на скорости 9600 Мбит/сек, но обмен данными будем осуществлять иначе.
Подключим модуль, как показано на рис. 8.17, временно отключим питание модуля
тумблером (показан на рис. 8.17 справа), загрузим в контроллер Arduino программу
из листинга 8.5, затем, отсоединив робота от компьютера и включив питание ESPмодуля, а затем и робота целиком, заставим его исполнить загруженный код.
Пояснение
Дело в том, что по умолчанию, модуль ESP-01 работает на другой скорости
(115 200 Мбит/сек), но такая скорость нам пока не нужна, и для единообразия скорость
нужно снизить, что делается командой AT+UART_DEF=9600,8,1,0,0. Для этого в контроллер и записывается программа, приведенная в листинге 8.5.
Листинг 8.5. Изменение скорости порта ESP-01
void setup() {
// Открыть сериал соединения:
Serial.begin(115200);
Delay(1000);

134

Глава 8

Serial.println(“AT+UART_DEF=9600,8,1,0,0”);
}
void loop() { }

Рис. 8.17. Схема подключения ESP-01 к Arduino UNO

Откроем в браузере сайт https://remotexy.com/ — это проект, позволяющий создавать заготовки программ управления для использования смартфона в качестве
пульта для устройств, имеющих беспроводные интерфейсы. На сайте потребуется
зарегистрироваться, после чего прямо там вы сможете создавать свои интерфейсы
управления. Но сначала нужно выбрать модули, соответствующие нашему проекту, — на рис. 8.18 они уже выбраны (справа). В качестве беспроводного интерфейса
выбран Wi-Fi access point (создание автономной сети Wi-Fi со своим паролем).
В качестве контроллера выбран тот контроллер, который мы применяем (на
рис. 8.18 — это Arduino UNO). В качестве платы беспроводного интерфейса выбран
ESP8266 Wi-Fi module — это тот самый ESP-01. Интерфейс подключения можно
выбрать из вариантов Software Serial и Serial — мы выбрали Serial.
Ниже показана настройка параметров точки доступа: имя (здесь RemoteXY) и пароль (здесь 12345678), значение порта не меняйте. Задайте и запомните эти данные,
они потребуются при подключении.
Затем нужно перейти в настройки элементов управления экрана и выбрать только
один элемент управления — джойстик (рис. 8.19). Настроим его параметры, как
показано в правой части рисунка (внизу): автоцентрирование, нет кнопки центрирования, нет кнопки G-сенсора.

Дистанционное управление роботом

135

Рис. 8.18. Настройка проекта в RemoteXY

Осталось нажать кнопку Получить исходный код — заготовка программы высвечивается в окне, и ее можно скопировать себе в проект.
В листинге 8.6 управление из RemoteXY уже вставлено — вы можете даже не создавать проект сами, а использовать именно этот, рекомендуется только настроить
название точки доступа и пароль:
#define REMOTEXY_WIFI_SSID "RemoteXY"
#define REMOTEXY_WIFI_PASSWORD "12345678"

Вам также понадобится скачать приложение RemoteXY для смартфона — оно есть
в магазинах приложений как Google, так и Apple.
Далее стандартным способом в среде Arduino IDE загрузите библиотеку поддержки
RemoteXY: Скетч | Подключить библиотеку | Управлять библиотеками, осуществите ее поиск и установку, после чего загрузите программу из листинга 8.6 в контроллер.

136

Глава 8

Рис. 8.19. Настройка элемента управления (джойстика) в RemoteXY

Теперь запускаем робота, запускаем приложение RemoteXY на смартфоне, добавляем в приложении новое устройство — нашего робота (интерфейс: Wi-Fi access
point, имя и пароль мы задали самостоятельно, — применяем эти настройки). Запускаем интерфейс и управляем роботом!
Листинг 8.6. Управление роботом (REMOTEXY)
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3

Дистанционное управление роботом
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Определение режима соединения и подключение библиотеки RemoteXY.
#define REMOTEXY_MODE__ESP8266_HARDSERIAL_POINT
#include <RemoteXY.h>
// Настройки соединения.
#define REMOTEXY_SERIAL Serial
#define REMOTEXY_SERIAL_SPEED 9600
#define REMOTEXY_WIFI_SSID "RemoteXY"
#define REMOTEXY_WIFI_PASSWORD "12345678"
#define REMOTEXY_SERVER_PORT 6377
// Конфигурация интерфейса.
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
// 19 bytes
{ 255, 2, 0, 0, 0, 12, 0, 16, 21, 1, 5, 32, 7, 26, 50, 50, 50, 189, 31 };
// Структура определяет все переменные и события
// вашего интерфейса управления.
struct {
// input variables
int8_t joystick_1_x; // oт -100 до 100
int8_t joystick_1_y; // oт -100 до 100
// other variable
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
// Инициализация.
void setup()
{
// Инициализируем удаленное управление.
RemoteXY_Init ();
// Инициализируем моторы.
motor_setup();
}
const unsigned long move_time = 300;
int Lmotor=0, Rmotor=0;
unsigned long _time = 0;
void loop()
{
RemoteXY_Handler();// Опрос удаленного управления.
// Если прошло 50 миллисек.
if ((millis() - _time) > 50)
{
if (RemoteXY.connect_flag == 1)

137

138

Глава 8
{
Lmotor = RemoteXY.joystick_1_y + RemoteXY.joystick_1_x;
Lmotor = constrain(Lmotor, -255, 255);
Rmotor = RemoteXY.joystick_1_y - RemoteXY.joystick_1_x;
Rmotor = constrain(Rmotor, -255, 255);
motors_power(Lmotor, Rmotor);
}
else
{
motors_power(0, 0);
}
_time = millis();

}
}

Выводы
В этой главе описаны способы дистанционного управления роботами на платформе
Arduino. Подробно рассмотрено управление роботом с помощью IR-канала, каналов Bluetooth и Wi-Fi. Подобрана элементная база, разработаны схемы подключения и программы управления для Bluetooth, Wi-Fi и для IR-канала, — теперь роботом можно управлять удаленно с любого IR-пульта (помните, что разные пульты
генерируют разные последовательности) и смартфона.
В следующей главе мы научим робота держать направление по гироприбору.

ГЛАВА 9
РОБОТ, ДЕРЖАЩИЙ НАПРАВЛЕНИЕ
ПО ЭЛЕКТРОННОМУ
ГИРОСКОПУ-АКСЕЛЕРОМЕТРУ

В этой главе мы научим робота держать направление как на месте, так и в движении. Оснастим робота комплексным электронным прибором MPU-6050, который
включает в себя гироскоп и акселерометр, а попутно раскроем принцип действия
этих устройств.
Для достижения поставленной цели нам потребуется решить следующие задачи:
1. Понять принцип работы гироскопа.
2. Понять принцип работы акселерометра.
3. Разобраться в различиях электронного и классического гироскопов.
4. Подключить электронный гироскоп-акселерометр MPU-6050 к Arduino.
5. Научиться получать данные с гироскопа-акселерометра MPU-6050.
6. Написать для робота программу, которая, используя данные, полученные
с MPU-6050, помогает роботу поворачивать на точные углы, держать направление при езде и ориентироваться в пространстве.

Гироскоп
Гироскоп — это прибор, способный реагировать на изменение ориентации объекта.
Простейшим примером гироскопа является волчок. Если волчок раскрутить на
перемещаемой поверхности — например, на доске (рис. 9.1, а), а затем наклонять
эту доску (рис. 9.1, б и в), то волчок будет пытаться сохранить свою ориентацию.
Гироскоп был изобретен в начале XIX века, и в конце того же века его впервые
применили на практике для стабилизации курса торпед. Более подробно о гироскопах можно узнать из специализированной литературы. Нам же пока необходимо
знать о нем лишь то, что гироскоп позволяет отследить углы отклонения устройства, на котором он установлен, от первоначального положения. Гироскопы бывают

140

Глава 9

как одноосевыми, так и трехосевыми, позволяющими получать данные сразу по
трем осям, различаются они по типам на роторные (классические гироскопы), вибрационные (сохраняющие направления своих колебаний) и оптические.

а

б

в

Рис. 9.1. Эффект гироскопа на примере волчка

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

ʦ̖
̡̯
̨̬

̦̌
̪

̬̌
̣̏

̖̦
̛́

ʦ̡̨̛̖̯̬̦̪̬̣̖̦̌̌̏́

а
Рис. 9.2. Применение гироскопа для измерения угла поворота

б

Робот, держащий направление по электронному гироскопу-акселерометру

141

Акселерометр
Акселерометр служит для измерения ускорения. С его помощью можно измерять
ускорение движущегося тела и, используя простые формулы, определять положение тела в пространстве. Например, если знать начальную скорость и считывать
ускорение в равные малые промежутки времени, несложно вычислить пройденное
расстояние (рис. 9.3). Рассмотрим это на примере прямолинейного движения:

L2 = L1 + Δt × V1 +

a2 × Δt 2
,
2

(9.1)

где L1 — расстояние, пройденное ранее; L2 — итоговое расстояние; Δt — приращение времени (время движения машинки от А до Б); V1 — начальная скорость
(в точке А); a 2 — полученное от акселерометра ускорение (полагаем на отрезке А–Б
ускорение постоянным). Точность измерения расстояния зависит от частоты сбора
данных и от вычислительных способностей процессора и измеряющего прибора.
А

Б

L1
L2

Рис. 9.3. Применение акселерометра для измерения пройденного расстояния

Второе предназначение акселерометра — это нахождение углов отклонения объекта от вертикали. Для точного измерения объект должен быть неподвижен или двигаться прямолинейно без ускорения. В этом случае на него действует только сила
притяжения, направленная к центру Земли.
На рис. 9.4 изображен неподвижный объект (робот) на наклонной плоскости. Координатная ось Y акселерометра робота направлена вперед, ось Z — вниз под прямым
углом к наклонной плоскости, ось X — к наблюдателю. Искомый угол отклонения
объекта от вертикали AngYZ находится по формуле
⎛a ⎞
AngYZ = arctg ⎜ Y ⎟ ,
⎝ aZ ⎠

(9.2)

где aY — проекция ускорения свободного падения на ось Y акселерометра; a Z —
проекция на ось Z. Можно также использовать формулы:
⎛a ⎞
AngYZ = arcsin ⎜ Y ⎟
⎝ g ⎠

142

Глава 9

или
⎛a
AngYZ = arccos ⎜ Z
⎝ g

⎞
⎟,
⎠

где g — ускорение свободного падения, но для электронного прибора, который будет рассмотрен далее, рекомендуется использовать формулу (9.2), оперирующую
только относительными величинами.

Пр
ое
на кция
ос
ьZ g

Пр
ое
на кция
ос
ьY g

AngYZ
Рис. 9.4. Нахождение угла отклонения от вертикали (робот неподвижен)

Электронный гироскоп
Электронный гироскоп серьезно отличается от классического — он вообще не показывает угол отклонения от заданного направления. Электронный гироскоп — это
прибор, регистрирующий и возвращающий текущую угловую скорость (скорость
вращения). Зная мгновенную угловую скорость, можно вычислить угол, на который
повернется объект за некоторый промежуток времени:

Ang 2 = Ang1 + Δt × Vang ,

(9.3)

где Ang1 — начальный угол, рассчитанный ранее; Δ t — малый промежуток времени, на котором можно считать, что угловая скорость не менялась; Vang — полученное от электронного гироскопа в течение Δ t значение угловой скорости;

Робот, держащий направление по электронному гироскопу-акселерометру

143

Ang 2 — рассчитанный угол, на который повернулся объект вокруг исследуемой
оси с начала поворота. На рис. 9.5 вращение происходит вокруг оси Z. Показания
гироскопа именно по этой оси следует использовать в расчетах.

Рис. 9.5. Расчет угла поворота по показаниям электронного гироскопа

Подключение
гироскопа-акселерометра MPU-6050
На рис. 9.6 показана микросхема гироскопа-акселерометра MPU-6050, установленная на печатную плату, что позволяет удобно его подключать. На плате имеется
8 контактов, но для работы достаточно подключить 4 из них: VCC — питание 3,3–5 В
(возьмем от платы Arduino), GND — «земля» (подключим к любому нулевому проводу), SDA и SCL — контакты шины I2C (подключаем их к соответствующим портам Arduino: A4 — SDA и A5 — SCL). Имеется на плате также информационный
красный светодиод, сигнализирующий о наличии питающего напряжения. Схема
подключения MPU-6050 к Arduino приведена на рис. 9.7.
Обмен данными между MPU-6050 и Arduino осуществляется по шине I2C, которая
представляет собой последовательный интерфейс. К шине I2C могут быть подключены одновременно несколько устройств, одно из которых должно являться ведущим, а остальные — ведомыми. В нашем случае ведущим будет контроллер
Arduinо, а модуль MPU-6050 — ведомым. Каждое устройство на шине I2C имеет
уникальный адрес. Когда ведущий начинает передачу, он передает по шине адрес

144

Рис. 9.6. Микросхема гироскопа-акселерометра MPU-6050, смонтированная на плате

Рис. 9.7. Схема подключения MPU-6050 к Arduino UNO

Глава 9

Робот, держащий направление по электронному гироскопу-акселерометру

145

устройства, к которому выполняется обращение. Устройства, подключенные к шине, проверяют этот адрес и в случае совпадения начинают обмен информацией.
Шина I2C имеет 2 контакта: SCL — вывод тактового сигнала, его задает ведущий,
и SDA — передача/прием данных в обоих направлениях. Для работы по шине I2C
потребуется соединить контакт SCL модуля с контактом SCL Arduino (порт A5) и
контакт SDA модуля с контактом SDA Arduino (порт A4), как показано на рис. 9.7.
Кроме обязательных контактов, плата MPU-6050 снабжена несколькими неиспользуемыми в нашем проекте, их назначение показано рис. 9.6.
На плате Arduino Sensor Shield v5.0, которую также можно использовать для подключения модуля MPU-6050, есть два выхода: для SDA (A4) и SCL (A5), разъемов
питания на 5 В тоже достаточно.

Получение данных с MPU-6050
Внимание!
Если в вашем роботе на порту Serial сейчас установлен Bluetooth-контроллер HC-06
или иное устройство удаленного управления, отключите его питание!

Листинг 9.1 реализует функцию получения и вывода данных от гироскопа-акселерометра MPU-6050. Его показания считываются с устройства и выводятся через
порт на компьютер:
 на рис. 9.8 представлены показания устройства, когда его ось Y приблизительно

совпадает с вертикалью и направлена вниз, при этом показания по этой оси наибольшие по модулю: –16 340...–16 220;

Рис. 9.8. Ось Y акселерометра перпендикулярна горизонту

146

Глава 9

 на рис. 9.9 представлены показания устройства, когда его ось Z вертикальна и

направлена вверх, — в этом случае наибольшие показания акселерометра снимаются по оси Z: 14 804...15 148.

Рис. 9.9. Ось Z акселерометра перпендикулярна горизонту

При вращении устройства изменяются проекции ускорения свободного падения на
его оси. В первом случае максимальна проекция на ось Y, во втором — на ось Z.
Показания устройства выводятся в относительных единицах, и их можно использовать для нахождения угла отклонения от вертикали по формуле (9.2).
Листинг 9.1. Получение данных от MPU-6050
// Библиотека для работы с протоколом I2C (порты A5/SCL и A4/SDA)
#include <Wire.h>
const int MPU_addr = 0x68; // Упрощенный
// I2C-адрес нашего гироскопа-акселерометра MPU-6050.
// Переменные для хранения данных, возвращаемых прибором.
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
//==============================================
void setup()
{
Wire.begin();
Wire.beginTransmission(MPU_addr);
// Производим запись в регистр энергосбережения MPU-6050.
Wire.write(0x6B);
Wire.write(0);
// Устанавливаем его в ноль.

Робот, держащий направление по электронному гироскопу-акселерометру

147

Wire.endTransmission(true);
Serial.begin(115200);
}
//================ НАЧАЛО ====================
void loop()
{
Wire.beginTransmission(MPU_addr);
// Готовим для чтения регистры с адреса 0x3B.
Wire.write(0x3B);
Wire.endTransmission(false);
// Запрос состояния 14 регистров.
Wire.requestFrom(MPU_addr, 14, true);
// 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcX = Wire.read() << 8 | Wire.read();
// 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcY = Wire.read() << 8 | Wire.read();
// 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
AcZ = Wire.read() << 8 | Wire.read();
// 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
Tmp = Wire.read() << 8 | Wire.read();
// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyX = Wire.read() << 8 | Wire.read();
// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyY = Wire.read() << 8 | Wire.read();
// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyZ = Wire.read() << 8 | Wire.read();
// Вывод в порт данных, полученных от прибора.
Serial.print(" AcX="); Serial.print(AcX);
Serial.print(" AcY="); Serial.print(AcY);
Serial.print(" AcZ="); Serial.println(AcZ);
Serial.print(" GyX="); Serial.print(GyX);
Serial.print(" GyY="); Serial.print(GyY);
Serial.print(" GyZ="); Serial.println(GyZ);
delay(1000);
}

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

148

Глава 9

Шкала значений MPU-6050
Как показания прибора из относительных единиц перевести в привычные величины? Для ответа на этот вопрос следует обратиться к документации. На сайте производителя InvenSense (www.invensense.com) представлена таблица основных характеристик MPU-6050 (рис. 9.10). В частности, чтобы перевести показания гироскопа
в градус/сек, их требуется поделить на 131. Для перевода ускорения в привычную
для нас систему мер требуется поделить показания прибора на 16 384, а затем
умножить на 9,8 м/с².

Рис. 9.10. Основные характеристики MPU-6050

Модернизация робота
Монтаж устройства
К этому моменту мы уже подключили акселерометр-гироскоп MPU6050 к контроллеру Arduino робота — теперь на двусторонний скотч приклеим его к корпусу, как
показано на рис. 9.11.

Программирование
Так как контроллеры Arduino UNO и Nano имеют малую вычислительную мощность, воспользуемся для производства вычислений микроконтроллером, который
встроен в MPU6050, — он возьмет на себя функции расчета углов наклона и поворота. При этом будем пользоваться уже готовыми расчетами. Это значительно разгрузит контроллер Arduino и позволит нам сосредоточить внимание на стабилизации поворотов.

Основные функции для получения углов
Подключим в листинге 9.2 библиотечный модуль <MPU6050dmp.h> (из библиотеки
mobrob3.zip, включенной в электронный архив, сопровождающий книгу), который
содержит функцию инициализации MPU6050 и инициализации его внутреннего

Робот, держащий направление по электронному гироскопу-акселерометру

149

Рис. 9.11. MPU-6050 установлен на корпусе робота

математического контроллера MPU6050_DMP_setup(), а также функцию опроса и сохранения углов YPR_loop().
Функция YPR_loop() возвращает true, если пришли новые данные, и false — в противном случае. Если новые данные пришли, то углы наклона/поворота (в радианах)
хранятся в массиве ypr: ypr[0] — поворот по горизонту (– влево, + вправо),
ypr[1] — наклон вперед/назад (– вперед, + назад), ypr[2] — наклон вправо/влево
(– наклон влево, + наклон вправо).
Листинг 9.2. Программа расчета углов поворота робота
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>
// Переменные для хранения поворота по осям.
float Yaw, Pitch, Roll;
void setup() {
Serial.begin(115200);
// Инициализация.
MPU6050_DMP_setup();
}
void loop() {
if (YPR_loop()) // Если пришли новые данные.

150

Глава 9

{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180 / M_PI;
Pitch = ypr[1] * 180 / M_PI;
Roll = ypr[2] * 180 / M_PI;
// Вывод в порт.
Serial.println("Y="+String(Yaw)+" P="+String(Pitch)+" R="+String(Roll));
}
}

После запуска программы нужно в течение пары минут оставить робота в покое —
за это время произведется пересчет смещения нуля гироскопа, что в дальнейшем
существенно повысит точность расчетов. Затем в порту появятся данные по углам
поворота (рис. 9.12).

Рис. 9.12. Углы поворота/наклона робота по осям

Стабилизация поворотов: ПИД-регулятор
Научим робота стабильно держать направление. Для этого применим пропорционально-интегрально-дифференцирующий регулятор (ПИД-регулятор), реализованный в программе, приведенной в листинге 9.3. ПИД-регулятор подает на моторы мощность для компенсации ошибки поворота исходя из трех составляющих
(звеньев):
 пропорциональная составляющая рассчитывается как разница между текущим

углом поворота и углом, на который требуется повернуть;
 интегральная составляющая рассчитывается как сумма ошибок поворота, по-

множенных на время действия этих ошибок (интеграл от ошибки по времени);

Робот, держащий направление по электронному гироскопу-акселерометру

151

 дифференцирующая составляющая рассчитывается как текущая скорость изме-

нения ошибки (от текущей ошибки угла отнимаем ошибку с прошлого опроса
и делим на время между опросами).
Для каждого из трех звеньев регулятора подбирается коэффициент пропорциональности с целью достижения максимальной точности поворота. MPU6050 производит
пересчет значений углов поворота не чаще 1 раза в 10 миллисекунд, но мы будем
опрашивать MPU 1 раз в 50 миллисекунд — этого достаточно с избытком для
успешного регулирования углов мощностью моторов.
Совет
Для стабильной работы робота (без зависаний) рекомендуется установить емкий
(1000 мкФ) электролитический конденсатор между контактами GND и 5V контроллера
Arduino. Без такого конденсатора возможны подвисания контроллера.

Самостоятельно изменяйте значения коэффициентов регулятора, постарайтесь сделать выводы о том, как изменение коэффициентов пропорциональности звеньев
регулятора влияет на устойчивость курса робота.
Листинг 9.3. Программа удержания роботом угла поворота
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Переменные для хранения поворота по осям.
float Yaw, Pitch, Roll;
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0, delta;
float dTime = 0;
float CommandYaw = 0;// Заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
float errorYaw = 0, olderrorYaw = 0;
// Скорость изменения ошибки поворота - дифференциальное звено регулятора.
float dErrorYaw = 0;
// Накопленная ошибка поворота - интегральное звено регулятора.
float iErrorYaw = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotPower = 0;
int Lmotor = 0, Rmotor = 0;
float Kp = 10.0;

152

Глава 9

float Ki = 15.0;
float Kd = 0.4;
void setup() {
// Инициализация.
MPU6050_DMP_setup();
// Инициализируем моторы.
motor_setup();
}
void loop() {
_millis = millis();
// Если пришли новые данные и время настало.
if ((time_out + 50) < _millis)
{
if (YPR_loop())
{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180 / M_PI;
// delta - время между вызовами в миллисекундах.
delta = _millis - time_out;
delta = constrain(delta, 10, 70); // Ограничим диапазон.
// dTime - время между вызовами (из миллисекунд в секундах).
dTime = float(delta) * 0.001;
time_out = _millis; // Сохраняем время вызова.
// Требуется стабилизировать только курс - используем Yaw.
// CommandYaw - заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
errorYaw = CommandYaw - Yaw;
while (errorYaw > 180.0) errorYaw -= 360.0; // Коррекция ошибки
while (errorYaw < -180.0) errorYaw += 360.0; // Коррекция ошибки
// Скорость изменения ошибки поворота - дифференциальное
// звено регулятора.
dErrorYaw = (errorYaw - olderrorYaw) / dTime;
olderrorYaw = errorYaw;// Запоминаем на будущее.
// Накопленная ошибка поворота - интегральное звено регулятора.
// Ki - коэффициент пропорциональности для iErrorYaw.
iErrorYaw = iErrorYaw + errorYaw * dTime * Ki;
// Ограничим интеральную составляющую.
iErrorYaw = constrain(iErrorYaw, -100.0, 100.0);
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
RotPower = errorYaw * Kp + dErrorYaw * Kd + iErrorYaw;
// Ограничим мощность реальным максимумом.
RotPower = constrain(RotPower, -255.0, 255.0);
Lmotor = int(RotPower);
Rmotor = -int(RotPower);
motors_power(Lmotor, Rmotor);
}
}
}

Робот, держащий направление по электронному гироскопу-акселерометру

153

Добавляем функцию движения
В предыдущей главе мы рассматривали три варианта управления движением робота: от IR-пульта, со смартфона с помощью Bluetooth-контроллера HC-05/06, а также
со смартфона с помощью установленной на роботе платы ESP-01 и программы удаленного управления на основе Wi-Fi (RemoteXY). Но этими тремя вариантами
возможности удаленного управления роботом не исчерпываются. Открою вам небольшую тайну: Bluetooth-контроллеры HC-05/06 тоже работают с приложением
RemoteXY! Поэтому рассмотрим новый — 4-й вариант — управления. При использовании HC-05/06 вам не придется ничего менять в роботе (только прошивку),
а при использовании ESP-01 изменения коснутся лишь блока организации связи.
Итак, создадим новый проект на сайте remotexy.com (рис. 9.13).
Ось Х джойстика отвечает за скорость поворота:
RotSpeed= float(RemoteXY.joystick_1_x) * 0.5

Тогда заданный угол будет рассчитываться по формуле
CommandYaw = CommandYaw + dTime * RotSpeed.

Рис. 9.13. Создание в приложении RemoteXY интерфейса управления для HC-05/06

154

Глава 9

Ось Y джойстика отвечает за скорость движения вперед-назад:
PowerMotor = float(RemoteXY.joystick_1_y) * 2.5

Остается поправить мощность, выдаваемую на моторы:
Lmotor = PowerMotor + int(RotPower) и Rmotor = PowerMotor - int(RotPower)
Листинг 9.4. Программа удержания роботом заданного угла поворота в движении
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Переменные для хранения поворота по осям.
float Yaw, Pitch, Roll;
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0, delta, _time = 0;
float dTime = 0;
float CommandYaw = 0;// Заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
float errorYaw = 0, olderrorYaw = 0;
// Скорость изменения ошибки поворота - дифференциальное звено регулятора.
float dErrorYaw = 0;
// Накопленная ошибка поворота - интегральное звено регулятора.
float iErrorYaw = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotPower = 0, RotSpeed = 0, PowerMotor = 0;
int Lmotor = 0, Rmotor = 0;
float Kp = 10.0;
float Ki = 15.0;
float Kd = 0.4;
// Далее относится к RemoteXY.
#define REMOTEXY_MODE__HARDSERIAL
#include <RemoteXY.h>
// Настройки соединения .
#define REMOTEXY_SERIAL Serial
#define REMOTEXY_SERIAL_SPEED 9600
#define REMOTEXY_ACCESS_PASSWORD "1234"

Робот, держащий направление по электронному гироскопу-акселерометру
// Конфигурация интерфейса.
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
// 19 bytes
{ 255, 2, 0, 0, 0, 12, 0, 16, 21, 1, 5, 32, 7, 26, 50, 50, 50, 189, 31 };
// Структура определяет все переменные и события вашего
// интерфейса управления
struct {
// input variables
int8_t joystick_1_x; // oт -100 до 100
int8_t joystick_1_y; // oт -100 до 100
// other variable
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
void setup() {
// Инициализация.
MPU6050_DMP_setup();
// Инициализируем моторы.
motor_setup();
// Инициализация удаленного управления.
RemoteXY_Init ();
}
void loop()
{
_millis = millis();
// Если время настало для опроса MPU.
if ((time_out + 50) < _millis)
{
if (YPR_loop()) // Если получены данные от MPU.
{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180 / M_PI;
// delta - время между вызовами в миллисекундах.
delta = _millis - time_out;
delta = constrain(delta, 10, 70); // Ограничим диапазон.
// dTime время между вызовами (из миллисекунд в секундах).
dTime = float(delta) * 0.001;
time_out = _millis; // Сохраняем время вызова.
// Расчет заданного угла поворота.
CommandYaw = CommandYaw + dTime * RotSpeed;
// Требуется стабилизировать только курс - используем Yaw.
// CommandYaw - заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
errorYaw = CommandYaw - Yaw;
while (errorYaw > 180.0) errorYaw -= 360.0; // Коррекция ошибки
while (errorYaw < -180.0) errorYaw += 360.0; // Коррекция ошибки

155

156

Глава 9
// Скорость изменения ошибки поворота - дифференциальное
// звено регулятора
dErrorYaw = (errorYaw - olderrorYaw) / dTime;
olderrorYaw = errorYaw;// Запоминаем на будущее.
// Накопленная ошибка поворота - интегральное звено регулятора.
// Ki - коэффициент пропорциональности для iErrorYaw.
iErrorYaw = iErrorYaw + errorYaw * dTime * Ki;
// Ограничим интеральную составляющую.
iErrorYaw = constrain(iErrorYaw, -100.0, 100.0);
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
RotPower = errorYaw * Kp + dErrorYaw * Kd + iErrorYaw;
// Ограничим мощность реальным максимумом
RotPower = constrain(RotPower, -255.0, 255.0);
Lmotor = PowerMotor + int(RotPower);
Rmotor = PowerMotor - int(RotPower);
Lmotor = constrain(Lmotor, -255.0, 255.0);
Rmotor = constrain(Rmotor, -255.0, 255.0);
motors_power(Lmotor, Rmotor);

}
}
RemoteXY_Handler();// Опрос удаленного управления.
// Если прошло 50 миллисек.
if (( _millis - _time) > 50)
{
// Если удаленное управление подключено!
if (RemoteXY.connect_flag == 1)
{
// Задаем скорость поворота по джойстику.
RotSpeed = float(RemoteXY.joystick_1_x) * 0.5;
// Задаем скорость движения по джойстику.
PowerMotor = float(RemoteXY.joystick_1_y) * 2.5;
}
else
{
RotSpeed = 0;
PowerMotor = 0;
}
_time = millis();
}
}

Думаю, эффект от работы системы движения робота вас не разочарует — робот
теперь умеет четко следовать по курсу, а это пригодится нам в следующих проектах.

Робот, держащий направление по электронному гироскопу-акселерометру

157

Выводы
В этой главе мы изучили принципы работы гироскопа и акселерометра. Разобрались с различиями между электронным и классическим гироскопами. Подключили
гироскоп-акселерометр MPU-6050 к Arduino, написали программу его опроса. Научили робота, используя ПИД-регулирование, точно удерживать направление как
в движении, так и на стоянке.
Таким образом, цель, поставленная нами перед собой в этой главе, достигнута.
Рекомендуем вам включить фантазию и самостоятельно модифицировать программу из листинга 9.4 — например, научить робота строить маршрут в виде сложных
геометрических фигур, отъезжать и точно возвращаться назад в место старта.

ГЛАВА 10
ДВИЖЕНИЕ
ПО ЧЕРНОЙ ЛИНИИ

Классической задачей для мобильных роботов является движение по линии. В нашем случае фон будет белым, а линия — черного цвета, шириной 4 см. Наш робот
должен двигаться по этой черной линии, не сбиваясь с курса.
Решение этой задачи предполагает:
 рассмотрение способов детектирования черной линии Arduino;
 обоснование выбора датчиков детектирования черной линии;
 подключение датчиков к роботу;
 составление алгоритма и практическую реализацию программы.

В качестве примера приведем типичную трассу для соревнований по биатлону для
роботов (рис. 10.1), которые традиционно проводят на робофестивалях. Робот должен начать движение от старта и, двигаясь только по черной линии, проехать, не
задев фишки, установленные в позициях 5–8, проезжая мимо, сбить фишки, установленные в позициях 1 и 2, и забрать с собой к финишу фишки, находящиеся
в позициях 3 и 4.

Рис. 10.1. Пример черной линии на соревнованиях роботов по биатлону

159

Движение по черной линии

Такую сложную задачу мы пока ставить перед собой не будем, а ограничимся только движением по линии.
Конечно, можно попытаться запрограммировать робота таким образом, чтобы он
четко знал, где ехать прямо, а где совершить поворот, но при этом малейшие непредвиденные обстоятельства: небольшая неровность, изменение шероховатости
покрытия, удар о препятствие — собьют робота с курса. Намного эффективнее
робот действует, если будет знать, где поверхность черная, а где светлая.

Обнаружение черной линии
Обнаружить черную линию нам помогут электронные приборы, которые реагируют
на изменение освещенности. Рассмотрим четыре таких прибора, наиболее применимых для решения поставленной задачи.

Фотодиод
Фотодиод (рис. 10.2, а) в темноте ведет себя как обычный диод — пропускает электрический ток только в одном направлении, а при освещении генерирует небольшой ток даже без источника питания. При обратном включении фотодиода он при
достижении освещенностью определенного уровня начинает пропускать электрической ток в обратном направлении — это позволяет использовать фотодиоды
в качестве датчиков наличия освещенности.

а

б

в

Рис. 10.2. Фотодиод (а) и схемы подключения фотодиода в качестве датчика наличия освещенности (б и в)

Схемы обратного включения фотодиодов приведены на рис. 10.2, б и в. Сигнал
снимается с выхода OUT, при этом для схемы на рис. 10.2, б наличие освещенности
характеризуется высоким выходным сигналом, а для схемы на рис. 10.2, в — низким. Подобные схемы не позволяют измерять уровень освещенности, а лишь фик-

160

Глава 10

сируют ее наличие, что говорит о необходимости подключения их к цифровым
портам, которые воспринимают только два вида сигналов: высокий — 1 и низкий — 0.
Чтобы фотодиод мог четко детектировать черную линию, нужно ее область подсветить, для чего, как правило, используется светодиод. Исходящий от него свет, отражаясь от светлой поверхности пола, попадает на фотодиод, вызывая в нем обратный ток. От черной же линии свет не отражается, что приводит к прекращению обратного тока фотодиода. Светодиод и фотодиод размещаются над исследуемой
поверхностью так, чтобы отраженный от нее свет светодиода попадал на фотодиод.
При этом расстояние до поверхности должно быть в пределах 1–2 см.
Фотодиоды и светодиоды производятся для разных спектров света, в том числе и
невидимого, — например, инфракрасного диапазона.

Фоторезистор
Фоторезистор (рис. 10.3, а) под действием света изменяет свое сопротивление. При
отсутствии освещения его сопротивление находится в пределах 1–200 МОм, а при
освещении — уменьшается на несколько порядков. При этом фоторезистор имеет
линейную зависимость сопротивления от освещенности, вследствие чего с его помощью удобно измерять освещенность в помещении.
Недостатками фоторезистора можно назвать достаточно высокое сопротивление и
инертность — он не реагирует на изменение освещенности с частотой выше 1 кГц.
Но при небольших скоростях движения робота использовать его все-таки можно.
Схемы подключения фоторезистора приведены на рис. 10.3, б и в. Подсветка фоторезистора светодиодом и размещение их над исследуемой поверхностью организуются таким же образом, как и в случае применения фотодиода.

а

б

в

Рис. 10.3. Фоторезистор (а) и схемы подключения фоторезистора
в качестве датчика освещенности (б и в)

161

Движение по черной линии

Фототранзистор
Фототранзистор (рис. 10.4, а) не только преобразует световой поток в электрический ток, подобно фотодиоду или фоторезистору, но и многократно его усиливает.
Визуально фототранзисторы могут не отличаться от фотодиодов, но подключаются
и работают иначе. Схемы подключения фототранзисторов приведены на рис. 10.4, б
и в. Основным преимуществом фототранзистора, как аналогового датчика, перед
фотодиодом, представляющим собой пороговый, или цифровой, датчик, является
возможность измерения уровня освещенности, что позволяет при определении цвета объекта фиксировать тона и полутона. Для этого следует подключать аналоговый выход OUT фототранзистора к аналоговым входам контроллера Arduino (A0–
A5 или A0–A7 — в зависимости от модели контроллера). При этом робот должен
будет экспериментально замерить уровень освещенности на белом фоне и уровень
освещенности на черной линии, а затем проанализировать полученные значения
в программе.
Подсветка фототранзистора светодиодом и размещение их над исследуемой
поверхностью организуются таким же образом, как и в случае применения фотодиода.

а

б

в

Рис. 10.4. Фототранзистор (а) и схемы подключения фототранзистора
в качестве датчика освещенности (б и в)

Инфракрасный датчик отражения TCRT 5000
В датчике TCRT 5000 (рис. 10.5) использована готовая связка инфракрасного светодиода и фототранзистора. На его выходах можно получать как аналоговый, так и
цифровой сигналы:
 цифровой выход датчика можно использовать для определения наличия черной

линии, при этом его чувствительность настраивается переменным резистором;

162

Глава 10

 на аналоговом выходе датчика интенсивность отраженного сигнала зависит от

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

ности, увеличивая точность срабатывания.

а

б
Рис. 10.5. Инфракрасный датчик отражения TCRT 5000: а — лицевая сторона; б — оборотная

163

Движение по черной линии

Цифровой выход датчика TCRT 5000 обозначен на плате как D0, а аналоговый —
как A0. Цифровой выход подключается к цифровому порту Arduino D0–D13, а аналоговый — к аналоговому A0–A5/A7. Контакт VCC подключается к питанию 5 В,
контакт GND — соответственно к «земле».
На плате датчика распаяны два информационных светодиода: зеленый, информирующий о наличии линии — срабатывании выхода D0, и красный, информирующий о наличии электропитания.
На плате контроллера Arduino аналоговые входы А0–А7 при программировании
в среде Arduino IDE имеют номера 14–21, но к ним можно обращаться и по имени
A0–A7. При этом A0–A5 могут работать и как обычные цифровые порты ввода/вывода. Выходы А6 и А7 распаяны не на всех платах, и они могут применяться
только для ввода.

* * *
В конечном итоге для определения наличия черной линии рекомендуем использовать именно инфракрасный датчик отражения TCRT 5000 — как наиболее удобное
и готовое решение, для которого мы и составим программы движения робота. При
этом мы воспользуемся аналоговым выходом датчика для реализации плавного
регулирования.

Подготовка робота: установка датчиков
При движении по непрерывной линии будет вполне достаточно двух датчиков —
справа и слева относительно бампера робота, чтобы на этой линии удержаться.
Датчики TCRT 5000, как правило, уже имеют припаянные контакты для разъемных
соединений. Для подключения датчиков через сервисную плату потребуются провода с разъемами «мама-мама», для подключения непосредственно к плате Arduino
UNO — провода «мама-папа», схема предлагаемых соединений приведена на
рис. 10.6.
Насадим датчик на длинный винт (рис. 10.7) и закрепим его сверху гайкой (гайка 1).
Чтобы датчик не вращался относительно оси винта, зафиксируем гайку термоклеем. Затем дополнительно накрутим на винт гайку (гайка 2), которая будет
в дальнейшем фиксировать датчик в корпусе снизу. То же проделаем и со вторым
датчиком
Подсоединим к датчикам провода и прикрепим датчики к роботу.
Для этого просверлим в передней части корпуса робота два отверстия — справа и
слева симметрично относительно центра на расстоянии 115 мм друг от друга
(рис. 10.8, а). Расстояние это зависит от ширины линии и от крутизны поворотов —
чем шире линиия и круче повороты, тем больше должно быть расстояние (у нас
ширина линии 40 мм). Вставим датчики винтами в просверленные отверстия и отрегулируем гайками фиксации к корпусу так, чтобы от поверхности корпуса они
отстояли на расстоянии 5–7 мм (рис. 10.8, б). Корпусные гайки рекомендуем также
зафиксировать термоклеем.

164

Рис. 10.6. Пара датчиков TCRT 5000: схема подключения

Рис. 10.7. Датчик TCRT5000 с винтом крепления: гайка 2 упирается в корпус робота снизу,
а сверху корпуса накручивается третья гайка

Глава 10

165

Движение по черной линии

а

б
Рис. 10.8. Робот с установленными датчиками TCRT 5000: а — вид на робота сверху;
б — вид на робота справа

Датчики должны получить электропитание 5 В и соединение с двумя аналоговыми
выводами Arduino. Воспользуемся для этого двумя свободными входами A1
и A2 — для левого и правого датчиков соответственно (см рис. 10.6).
Подключив датчики, желательно провести тестирование правильности их подключения к роботу. Для этого следует:

166

Глава 10

1. Загрузить в робот программу из листинга 10.1.
2. Не включая питания HC-05 или иного модуля удаленного доступа, запустить
монитор порта.
3. Подкладывая под датчики робота черные и белые листы, проследить изменение
состояния датчиков на экране компьютера по монитору порта Arduino.
На экране будет появляться информация (данные от 0 до 1024) для пары датчиков — это считанный аналоговый сигнал. Если на белой линии числа колеблются
от 0 до 200, а на черной стабильно больше 400, значит, датчики подключены верно.
Листинг 10.1. Тестовая программа проверки подключения датчиков линии
void setup() {
pinMode(A1, INPUT); // Не обязательно.
pinMode(A2, INPUT); // Не обязательно.
Serial.begin(115200);
}
void loop() {
Serial.println("A1="+String(analogRead(A1))+" A2="+String(analogRead(A2)));
delay(200);
}

Упрощенная программа движения
Теперь рассмотрим алгоритм, которого необходимо придерживаться при следовании по извилистой линии (рис. 10.9). Считаем, что в начале движения линия находится между правым и левым датчиком, — т. е. робот установлен на линию. Пока
оба датчика показывают состояние «светлый фон», робот будет ехать прямо.
Если левый датчик показал «черный фон», робот должен повернуть налево. Если
правый датчик показал «черный фон», робот должен повернуть направо.
Программа движения робота с учетом указанных обстоятельств приведена в листинге 10.2. Состояние датчиков снимается через 50 миллисекунд: if ((time_out +
50) < _millis), но можно уменьшить время между опросами — это даст небольшой прирост точности. Робот довольно тяжелый и может по инерции проскочить
поворот и сбиться с линии. Поэтому следует отрегулировать скорость его движения
(в программе мощность PowerMotor = 50). Также важно, чтобы в случае осуществления поворотов робот подтормаживал: if (RotSpeed != 0.0) PowerMotor = -10.
Вы наверное заметили, что в программе предусмотрено подключение модуля удаленного управления RemoteXY. Зачем ручное управление, если робот движется
автономно? Это сделано для демонстрации еще одной полезной функции
RemoteXY — подключившись к роботу удаленно (через приложение RemoteXY),
вы сможете проследить графики значений, снимаемых с датчиков.

Движение по черной линии

Рис. 10.9. Простейший алгоритм движения по черной линии

Листинг 10.2. Движение робота по черной линии
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotSpeed = 0, PowerMotor = 0;
int Lmotor = 0, Rmotor = 0;

167

168

Глава 10

// Коэффициенты регулятора для режима удержания угла.
int LeftFotoTrans = 0, RightFotoTrans = 0;
#define GPIOLeftFotoTrans A1
#define GPIORightFotoTrans A2
#define GPIOFotoLight 12
float RotPower2 = 0, RotPower1 = 0;
// Далее относится к RemoteXY.
// Определение режима соединения и подключение библиотеки RemoteXY.
#define REMOTEXY_MODE__HARDSERIAL
#include <RemoteXY.h>
// Настройки соединения.
#define REMOTEXY_SERIAL Serial
#define REMOTEXY_SERIAL_SPEED 9600
#define REMOTEXY_ACCESS_PASSWORD "1234"
// Конфигурация интерфейса.
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
// 27 bytes
{ 255, 2, 0, 8, 0, 21, 0, 16, 21, 1, 5, 32, 8, 7, 50, 50, 50, 189, 31, 68,
18, 7, 61, 49, 35, 24, 31, 135
};
// Структура определяет все переменные и события вашего
// интерфейса управления
struct {
// input variables
int8_t joystick_1_x; // oт -100 до 100
int8_t joystick_1_y; // oт -100 до 100
// output variables
float onlineGraph_1_var1;
float onlineGraph_1_var2;
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
void setup() {
// Инициализируем моторы.
motor_setup();
// Инициализация удаленного управления.
RemoteXY_Init ();
pinMode(GPIOLeftFotoTrans, INPUT);
pinMode(GPIORightFotoTrans, INPUT);
}
void loop()
{
RemoteXY_Handler();// Опрос удаленного управления.
_millis = millis();
// Если время настало для опроса MPU.
if ((time_out + 50) < _millis)

Движение по черной линии

169

{
time_out = _millis; // Сохраняем время вызова.
LeftFotoTrans = analogRead(GPIOLeftFotoTrans);
RightFotoTrans = analogRead(GPIORightFotoTrans);
RotSpeed = 0;
// Упрощенный алгоритм дискретного определения
// черной линии
// если >400 линия есть, иначе – нет.
// Поворот влево больше, чем вправо.
// Благодаря этому робот будет все время "крениться" влево // это нужно для обхода против часовой стрелки.
if (LeftFotoTrans > 400) RotSpeed -= 100;
if (RightFotoTrans > 400) RotSpeed += 50;
RemoteXY.onlineGraph_1_var1 = LeftFotoTrans;
RemoteXY.onlineGraph_1_var2 = RightFotoTrans;
// Мощность движения вперед - можно изменять.
PowerMotor = 50;
// Если есть составляющая поворота, то тормозим.
if (RotSpeed != 0.0) PowerMotor = -10;
// Расчетная мощность на моторы.
Lmotor = PowerMotor + int(RotSpeed);
Rmotor = PowerMotor - int(RotSpeed);
// Ограничиваем значения.
Lmotor = constrain(Lmotor, -255.0, 255.0);
Rmotor = constrain(Rmotor, -255.0, 255.0);
motors_power(Lmotor, Rmotor);
}
}

Используем аналоговые данные
«на всю катушку»
Мы уже задействовали ПИД-регулятор, когда удерживали робота по направлению,
используя данные с MPU6050 (см. разд. «Стабилизация поворотов» главы 9). Полагаю, вам понравились результаты! А что если применить подобный подход для
уточнения положения робота относительно черной линии? У нас же есть аналоговые данные, анализируя которые мы можем не просто зафиксировать факт пересечения линии, но даже определить состояние, когда к ней приближаемся, и датчик
только «немного темнеет»!
Для организации подобного регулирования нам требуется некая ошибка регулирования — разница между показаниями правого (RightFotoTrans) и левого
(LeftFotoTrans) датчиков как раз подойдет:
ErrorFotoRotation = (RightFotoTrans - LeftFotoTrans)

Это пропорциональная составляющая регулятора.

170

Глава 10

Скорость, с которой эта ошибка изменяется, будет дифференцирующей составляющей:
dErrorFotoRotation=(ErrorFotoRotation - oldErrorFotoRotation)/dTime,

где oldErrorFotoRotation — ошибка от прошлого опроса, dTime — время, прошедшее с прошлого опроса датчиков.
Накопленная ошибка:
iErrorFotoRotation = iErrorFotoRotation + ErrorFotoRotation*dTime,

есть интегрирующая составляющая регулятора.
Остается задать для звеньев регулятора коэффициенты пропорциональности влияния на результат — и поехали!
Внимательно изучив программу (листинг 10.3) и испытав ее в работе, вы могли заметить, что мощность переднего хода существенно увеличена, исчезла остановка
при поворотах, робот движется значительно быстрее и почти не ошибается.
В программе также есть вывод данных на смартфон. Самостоятельно измените
программу так, чтобы на смартфон выводился только график текущей мощности
регулирования.
Листинг 10.3. Движение робота по черной линии
с ПИД-регулированием мощности поворотов
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotSpeed = 0, PowerMotor = 0;
int Lmotor = 0, Rmotor = 0;
// Коэффициенты регулятора для режима удержания угла.
int LeftFotoTrans = 0, RightFotoTrans = 0;
#define GPIOLeftFotoTrans A1
#define GPIORightFotoTrans A2
#define GPIOFotoLight 12
// Далее относится к RemoteXY.
// Определение режима соединения и подключение библиотеки RemoteXY.
#define REMOTEXY_MODE__HARDSERIAL
#include <RemoteXY.h>

Движение по черной линии

171

// Настройки соединения.
#define REMOTEXY_SERIAL Serial
#define REMOTEXY_SERIAL_SPEED 9600
#define REMOTEXY_ACCESS_PASSWORD "1234"
// Конфигурация интерфейса.
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
// 27 bytes
{ 255, 2, 0, 8, 0, 21, 0, 16, 21, 1, 5, 32, 8, 7, 50, 50, 50, 189, 31, 68,
18, 7, 61, 49, 35, 24, 31, 135
};
// Структура определяет все переменные и события вашего
// интерфейса управления
struct {
// input variables
int8_t joystick_1_x; // oт -100 до 100
int8_t joystick_1_y; // oт -100 до 100
// output variables
float onlineGraph_1_var1;
float onlineGraph_1_var2;
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
void setup() {
// Инициализируем моторы.
motor_setup();
// Инициализация удаленного управления.
RemoteXY_Init ();
pinMode(GPIOLeftFotoTrans, INPUT);
pinMode(GPIORightFotoTrans, INPUT);
}
void loop()
{
RemoteXY_Handler();// Опрос удаленного управления.
_millis = millis();
// Если время настало для регулирования.
if ((time_out + 10) < _millis)
{
// Коэффициенты пропорциональности ПИД-регулятора.
float Kfr_p = 0.9; // Коэф. пропорциональное звено (пропорционально ошибке)
float Kfr_d = 0.03; // Коэф. дифференцирующее звено (проп. скорости)
float Kfr_i=0.25;
// Коэф. интегрирующее звено (проп.
// накопленной ошибке)
// Время в секундах.
float dTime = float(_millis - time_out)*0.001;
// Ограничиваем диапазон времени.
dTime = constrain (dTime, 0.01,0.06);
time_out = _millis; // Сохраняем время вызова.

172

Глава 10
static float oldErrorFotoRotation = 0;
static float iErrorFotoRotation = 0;
static int oldRotSpeed =0;
// Снимаем данные c датчиков, запоминаем их в переменные.
// LeftFotoTrans и RightFotoTrans
// Для левого датчика немного смещаем значение,
// чтобы робот двигался со смещением влево
// относительно черной линии (если обход по часовой стрелке)
LeftFotoTrans = analogRead(GPIOLeftFotoTrans)+50;
RightFotoTrans = (analogRead(GPIORightFotoTrans));
// Расчет ошибки положения относительно линии
float ErrorFotoRotation = (RightFotoTrans - LeftFotoTrans);
// Расчет скорости изменения ошибки.
float dErrorFotoRotation=(ErrorFotoRotation - oldErrorFotoRotation)/dTime;
// Расчет интеграла от ошибки по времени.
iErrorFotoRotation += ErrorFotoRotation*dTime;
// Ограничение интегральной части.
iErrorFotoRotation = constrain(iErrorFotoRotation,-50,50);
// Работаем с целыми числами.
// Расчет ПИД-регулятора: пропорционально ошибке линии,
// пропорционально скорости изменения ошибки линии,
// пропорционально интегралу ошибки линии по времени.
// (Сумма ошибок, помноженных на время действия этих ошибок).
RotSpeed = ErrorFotoRotation *Kfr_p + dErrorFotoRotation *Kfr_d +
iErrorFotoRotation*Kfr_i;
// Ограничиваем мощность поворота.
RotSpeed = constrain(RotSpeed,-255,255);
// Запоминаем старое значение ошибки линии.
oldErrorFotoRotation = ErrorFotoRotation;
// Передаем данные с датчиков в смартфон.
RemoteXY.onlineGraph_1_var1 = LeftFotoTrans;
RemoteXY.onlineGraph_1_var2 = RightFotoTrans;
// Задаем мощность движения вперед.
PowerMotor = 90;
// Рассчитываем мощность, выдаваемую на каждый мотор.
Lmotor = PowerMotor + RotSpeed;
Rmotor = PowerMotor - RotSpeed;
// Ограничиваем мощность на моторы.
Lmotor = constrain(Lmotor, -255, 255);
Rmotor = constrain(Rmotor, -255, 255);
// Управляем моторами.
motors_power(Lmotor, Rmotor);

}
}

Движение по черной линии

173

Выводы
В этой главе мы рассмотрели ряд датчиков освещенности, выбрали наиболее подходящий и оснастили им нашего робота. Теперь он может самостоятельно уверенно
и практически безошибочно двигаться по черной линии (рис. 10.10).

Рис. 10.10. Робот, двигающийся по черной линии

А теперь переходим к следующему проекту, которому посвящена глава 11, — разберемся с работой ультразвукового датчика расстояния и научим робота вращать
головой, как локатором.

ГЛАВА 11
ИЗМЕРЕНИЕ РАССТОЯНИЯ
И ПОВОРОТНАЯ ГОЛОВА

Сделаем робота более интеллектуальным, поместив на него дальномер и научив
реагировать на изменение его показаний.

Измеряем расстояние
Для проекта Arduino адаптировано довольно много датчиков расстояния. Датчики
различаются:
 по физическим принципам измерения:

• инфракрасные (по углу отраженного луча);
• лазерные (по сдвигу фазы или времени до момента возврата отражения);
• ультразвуковые (эхолокация);
 по интерфейсу передачи данных:

• аналоговые (генерируют на выходе напряжение определенной величины,
пропорциональное расстоянию);
• цифровые (передают расстояние в виде числа);
 по диапазону измеряемого расстояния;
 по скорости работы;
 по степени защиты от воздействий окружающей среды:

• только для помещений;
• для уличного использования;
• для агрессивных сред.
Рассмотрим несколько из них.

Измерение расстояния и поворотная голова

175

Инфракрасный датчик расстояния
GP2Y0A021YK0F
Инфракрасные датчики расстояния мы рассмотрим на примере наиболее надежного, по мнению автора, датчика GP2Y0AХХХХХХ (рис. 11.1). Эта серия датчиков
включает в себя три изделия, которые отличаются только диапазоном измеряемых
расстояний:
 датчик GP2Y0A41SK0F: 4–30 см;
 датчик GP2Y0A021YK0F: 10–80 см;
 датчик GP2Y0A02YK0F: 20–150 см.
Выводят датчики информацию в виде нелинейного аналогового сигнала. Величина
выходного напряжения считывается функцией analogRead(GPIO), а затем пересчитывается по формуле
L( x) = 32 × ( x × 0.0048828125)−1.1 ,

где L — расстояние до препятствия в миллиметрах; x — значение от 0 до 1024, полученное с аналогового входа от датчика.
Выход датчика обязательно должен быть подключен к сигнальному входу Arduino,
который имеет возможность считывать аналоговый сигнал (для Arduino UNO и
Nano это контакты A0–A7). Как показано на рис. 11.2, для подключения датчика
выбран контакт A3.

Рис. 11.1. Инфракрасный датчик
расстояния

Рис. 11.2. Инфракрасный датчик расстояния подключен
к контакту A3

176

Глава 11

Загрузим в плату Arduino скетч из листинга 11.1, предварительно отключив питание от устройств удаленного управления, чтобы они не мешали записывать скетч,
а затем выводить показания через порт на компьютер.
Листинг 11.1 Тест датчика GP2Y0A021YK0F
// Объявляем константу|макроподстановку GPIOsharp со значением A3
#define GPIOsharp A3
void setup() {
// Открываем соединение с ПК на скорости 115 200.
// Проверьте/установите скорость монитора порта!!!
Serial.begin(115200);
}
// Создаем переменную, в которой храним время в миллисекундах
// предыдущего измерения.
// Переменная беззнаковая до 4294967295.
unsigned long Old_dataTime = 0;
void loop() {
// Если подошло время (через 300 миллисек), выполняем измерение.
if (millis() - Old_dataTime > 300)
{
// Создаем переменную и записываем в нее значение сигнала.
float volume = analogRead(GPIOsharp);
// Умножаем полученное значение на коэффициент 0.0048828125.
volume = volume * 0.0048828125;
// Возводим volume в отрицательную степень -1.10,
// а затем умножаем на 32.
volume = 32.0 * pow(volume, -1.10);
// Запоминаем текущее время в миллисекундах.
Old_dataTime = millis();
// Вывод дистанции в Serial-порт на компьютер.
Serial.print("distance= "); Serial.println(volume);
}
}

Лазерный датчик расстояния VL53L1X
В своих проектах разумно использовать датчики, которые несложно приобрести —
в том числе на известной китайской торговой площадке. К числу таких датчиков
относится и лазерный дальномер VL53L1X. Датчик измеряет расстояние от 4 сантиметров до 4 метров и стабильно работает при разной освещенности. На рис. 11.3
приведен пример его подключения по интерфейсу I2C на общую шину с гироприбором MPU6050. Форм-факторы предлагаемых в продаже версий этого датчика
очень различаются, поэтому не старайтесь найти именно тот, который изображен
на рисунке, — ищите по названию и интерфейсу подключения. И не путайте
VL53L1X с VL53L0X — это инфракрасный датчик!

Измерение расстояния и поворотная голова

177

Рис. 11.3. Подключение лазерного датчика расстояния VL53L1X

Перед использованием датчика следует с помощью менеджера библиотек Arduino
IDE установить библиотеку VL53L1X от SparkFun. В среде Arduino IDE появится
множество содержащих подробные описания примеров по использованию этого
датчика — можете протестировать их самостоятельно.

Ультразвуковые дальномеры
HC-SR04 и US-026/25
Наиболее доступным видом датчиков расстояния являются ультразвуковые: на
сегодняшний день это классические сонары HC-SR04 (рис. 11.4) и относительно
новые US-026/25. Различия у них небольшие: HC-SR04 питается только от 5 вольт,
а US-026/25 — от 3 до 5 вольт, HC-SR04 может измерять расстояние от 2 см до
4 метров, а US-026/25 — от 2 см до 6 метров. Для нашего робота нет разницы, какой датчик: HC-SR04 или US-026/25 — будет применен.
Питание 5 вольт для дальномера следует взять с контактов Arduino +5V и GND.
Под дальномер HC-SR04 на плате Arduino будут заняты два GPIO: D13 (Trig) и D12
(Echo). На Trig (D13) подается сигнал начала измерения, после чего дальномер генерирует ультразвуковой сигнал, который распространяется в воздухе и отражается
от препятствий, формируя обратный сигнал, который и ловит дальномер, сигнализируя об этом событии на контакте Echo (D12). Анализируя время от запуска

178

Глава 11

Рис. 11.4. Ультразвуковой дальномер HC-SR04: слева — вид спереди; справа — вид сзади

сигнала до получения отраженного сигнала и зная скорость распространения звука
в воздухе, мы можем рассчитать расстояние до препятствия.
Подключим сонар HC-SR04 к роботу, а робота — к компьютеру и протестируем
возможности измерения расстояния. Для этого подойдет программа из листинга 10.2. В ней дальномеру назначаются порты 13 и 12, создается связь с ПК, а в основной программе с периодом 300 миллисекунд измеряется расстояние до препятствия и полученное значение передается в порт ПК. В оболочке Arduino IDE следует открыть монитор порта — тогда в окне монитора будет отображаться расстояние
(рис. 11.5). Максимальное измеряемое расстояние в примере — 300 см.

Рис. 11.5. Отображение измеренного расстояния в окне монитора порта

Листинг 11.2. Программа измерения расстояний
// Подключаем библиотеку, управляющую дальномером.
#include <mobrob3sonar.h>
void setup()

Измерение расстояния и поворотная голова

179

{
// Инициализируем дальномер Trig = 13, Echo = 12.
Sonar_init(13, 12);
// При инициализации задаем скорость порта для связи с ПК.
Serial.begin(115200); // start the serial port
}
// Создаем переменную, в которой храним время в миллисекундах
// предыдущего измерения.
// Переменная беззнаковая до 4294967295.
unsigned long Old_dataTime = 0;
void loop()
{
// Если подошло время (через 300 миллисек), выполняем измерение.
if (millis() - Old_dataTime > 300)
{
// Получаем дистанцию в сантиметрах с лимитом 300 см.
int prepyatstvie = Sonar(300);
Serial.print("Distance="); // Оформляем вывод.
Serial.print(prepyatstvie); // Выводим дистанцию.
Serial.println(" cm.");
// Оформляем вывод.
// Запоминаем текущее время в миллисекундах
Old_dataTime = millis();
}
}

Поиск препятствия
Чтобы измерить расстояние до препятствий с разных сторон, роботу не обязательно
иметь поворотную голову, — ведь мы научили его делать точные повороты, а значит, можно сканировать пространство, вращая самого робота. Давайте попробуем
это сделать. Пусть наш робот вращается, измеряет расстояние и передает угол поворота и расстояние до препятствия на смартфон. Для этого разработаем интерфейс
в приложении RemoteXY, взяв за основу пример из разд. «Стабилизация поворотов: ПИД-регулятор» главы 9, но изменим в нем интерфейсную часть (рис. 11.6).
При этом немного поменяем код так, чтобы робот поворачивался на 5 градусов,
после завершения поворота сканировал расстояние и поворачивался дальше, а расстояние и угол выводил на смартфон, запоминая угол, при котором расстояние до
препятствия было наименьшим.
Программа из листинга 11.3 ждет, когда будет подключен смартфон. Затем, при
установке в интерфейсе RemoteXY на смартфоне переключателя в положение ON,
робот начнет вращаться с малой скоростью, одновременно измеряя расстояние.
Расстояние и угол будут выведены на экран. Если переключатель установить в положение OFF, робот развернется на тот угол, где расстояние до препятствия было
наименьшим.
На рис. 11.7 представлен результат работы программы сканирования на экране
смартфона. Когда робот повернут на небольшой угол, он производит измерение
и выдает его на нижний стрелочный индикатор, верхний индикатор показывает
текущий угол поворота.

180

Глава 11

Рис. 11.6. Создаем интерфейс, показывающий текущий угол поворота и расстояние до препятствия

Листинг 11.3. Программа сканирования расстояния до робота
с выводом на экран смартфона
// Подключаем библиотеку, управляющую дальномером.
#include <mobrob3sonar.h>
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3

Измерение расстояния и поворотная голова
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Переменные для хранения поворота по осям.
float Yaw, Pitch, Roll;
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0, delta, _time = 0;
float dTime = 0;
float CommandYaw = 0;// Заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
float errorYaw = 0, olderrorYaw = 0;
// Скорость изменения ошибки поворота - дифференциальное звено регулятора.
float dErrorYaw = 0;
// Накопленная ошибка поворота - интегральное звено регулятора.
float iErrorYaw = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotSpeed = 0, RotPower = 0;
int Lmotor = 0, Rmotor = 0, PowerMotor = 0, integRotPower = 0;
int Temp_var, ActualSonar;
float ActualAngle=0;
uint8_t Old_switch_1=0;
// Коэффициенты регулятора для режима удержания угла.
float Kp = 20.0;
float Ki = 15.0;
float Kd = 0.4;
// Далее относится к RemoteXY.
// Определение режима соединения и подключение библиотеки RemoteXY.
#define REMOTEXY_MODE__HARDSERIAL
#include <RemoteXY.h>
// Настройки соединения.
#define REMOTEXY_SERIAL Serial
#define REMOTEXY_SERIAL_SPEED 9600
#define REMOTEXY_ACCESS_PASSWORD "1234"
// Конфигурация интерфейса.
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
// 91 bytes
{ 255,1,0,4,0,84,0,16,31,1,2,0,23,4,22,11,2,26,31,31,
79,78,0,79,70,70,0,71,56,11,18,44,44,16,2,24,255,0,0,0,
0,0,0,180,67,0,0,240,65,0,0,0,0,0,0,0,0,24,0,71,
56,13,64,38,38,16,2,24,111,0,0,0,0,0,0,150,67,0,0,32,
66,0,0,0,0,0,0,0,0,24,0 };
// Структура определяет все переменные и события вашего
// интерфейса управления
struct {
// input variables
uint8_t switch_1; // =1 если переключатель включен и =0 если отключен

181

182
// output variables
int16_t instrument_1; // oт 0 до 360
int16_t instrument_2; // oт 0 до 300
// other variable
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
void setup()
{
// Инициализация гироскопа.
MPU6050_DMP_setup();
// Инициализируем моторы.
motor_setup();
// Инициализация удаленного управления.
RemoteXY_Init ();
// Инициализируем дальномер Trig = 13, Echo = 12.
Sonar_init(13, 12);
}
void loop()
{
_millis = millis();
if ((time_out + 11) < _millis)
{
if (YPR_loop()) // Если получены данные от MPU.
{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180 / M_PI;
// delta - время между вызовами в миллисекундах.
delta = _millis - time_out;
delta = constrain(delta, 10, 70); // Ограничим диапазон.
// dTime - время между вызовами (из миллисекунд в секундах)
dTime = float(delta) * 0.001;
time_out = _millis; // Сохраняем время вызова.
// Расчет заданного угла поворота.
// Требуется стабилизировать только курс - используем Yaw.
// CommandYaw - заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
errorYaw = CommandYaw - Yaw;
if (errorYaw > 180.0) errorYaw -= 360.0; // Коррекция ошибки.
if (errorYaw < -180.0) errorYaw += 360.0; // Коррекция ошибки.
// Скорость изменения ошибки поворота – дифференциальное
// звено регулятора.
dErrorYaw = (errorYaw - olderrorYaw) / dTime;
olderrorYaw = errorYaw; // Запоминаем на будущее.
// Накопленная ошибка поворота - интегральное звено регулятора.
// Ki - коэффициент пропорциональности для iErrorYaw.
iErrorYaw = iErrorYaw + errorYaw * dTime * Ki;

Глава 11

Измерение расстояния и поворотная голова
// Ограничим интегральную составляющую.
iErrorYaw = constrain(iErrorYaw, -100.0, 100.0);
// Мощность, передаваемая на моторы
// для компенсации ошибки поворота.
RotPower = errorYaw * Kp + dErrorYaw * Kd + iErrorYaw;
// Если мы на месте: ошибка меньше 0.3 градуса
// и скорость поворота <0.5 градусов/сек
if ((fabs(errorYaw) < 2.0) && (fabs(dErrorYaw) < 0.5))
{
// Если смартфон подключен и включено сканирование.
if (RemoteXY.connect_flag == 1)
{
RemoteXY.instrument_1 = Yaw;
if (RemoteXY.switch_1 == 1)
{
if(Old_switch_1!=RemoteXY.switch_1)
{
Old_switch_1=RemoteXY.switch_1;
ActualSonar = 300;
}
// Вычисляем расстояние и записываем в переменную.
Temp_var = Sonar(300);
// Текущий угол во второй график.
if (Temp_var < ActualSonar) {
ActualSonar = Temp_var;
ActualAngle = Yaw;
}
RemoteXY.instrument_2 = Temp_var;
CommandYaw += 5; // Вращаем робота на 5 градусов
if (CommandYaw > 180.0) CommandYaw -= 360.0; // Коррекция
// угла+-180.
if (CommandYaw < -180.0) CommandYaw += 360.0; // Коррекция
// угла+-180.
}
else
{
if(Old_switch_1!=RemoteXY.switch_1)
{
Old_switch_1=RemoteXY.switch_1;
CommandYaw = ActualAngle;
}
else
{
ActualSonar = Sonar(300);
RemoteXY.instrument_2 = ActualSonar;
}
}
}
}

183

184

Глава 11
// Ограничим мощность реальным максимумом.
integRotPower = constrain(int(RotPower), -255, 255);
Lmotor = PowerMotor + integRotPower;
Rmotor = PowerMotor - integRotPower;
Lmotor = constrain(Lmotor, -255, 255);
Rmotor = constrain(Rmotor, -255, 255);
motors_power(Lmotor, Rmotor);

}
}
RemoteXY_Handler(); // Опрос удаленного управления.
}

Рис. 11.7. Робот и интерфейс смартфона: верхний стрелочный индикатор указывает
на текущий угол поворота в градусах, нижний — на расстояние до препятствия в сантиметрах

Поворотная голова
Если датчик расстояния поместить на вал сервомотора, то можно сканировать пространство без поворота самого робота.
Существует разновидность сервоприводов — сервомашинки (рис. 11.8), они управляются ШИМ-сигналом на частоте 50 Гц и предназначены для поворота вала на
угол, пропорциональный длительности импульса ШИМ-сигнала. Подобные моторы
широко используют в любительских устройствах — в частности, в летательных
аппаратах для управления рулями высоты.

Измерение расстояния и поворотная голова

185

Рис. 11.8. Девятиграммовая сервомашинка

Схемы подключения
Питание сервомашинки и управление ею осуществляются через три провода: красный — «плюс» питания (от 3 до 7 вольт), коричневый — «минус» питания (GND),
оранжевый или желтый — вход сигнала управления от микроконтроллера.
Монтажная схема подключения дальномера и поворотной сервомашинки к плате
Arduino приведена на рис. 11.9. Как здесь показано, управление сервомашинкой
осуществляется от GPIO A0.

Рис. 11.9. Монтажная схема подключения сервомотора и дальномера

186

Глава 11

Электропитание сервомашинки не должно производиться от контакта 5 вольт
Arduino — это может привести к выходу из строя стабилизатора платы Arduino или
к сбоям в работе программы. Если ваш драйвер моторов реализован на основе
L298N, можно взять 5 вольт от него (рис. 11.10). Если робот запитан двумя после

Рис. 11.10. Питание сервомашинки: вариант с L298N

Рис. 11.11. Питание сервомашинки через понижающий отдельный стабилизатор

Измерение расстояния и поворотная голова

187

довательно соединенными аккумуляторами 18650 (7,4 вольта), используйте отдельный понижающий стабилизатор (рис. 11.11). Если же вы используете один аккумулятор 18650 (3,7 вольт), то сервомашинки SG90 и MG90S могут быть запитаны
напрямую от этого источника питания (рис. 11.12).
Под дальномер HC-SR04 на плате Arduino заняты два порта: D13 (Trig) и D12
(Echo). Управление сервомашинкой, поворачивающей голову робота, как уже отмечалось ранее, будет производиться с порта A0 платы Arduino. Порт A0 хотя
и является аналоговым, но позволяет работать и в цифровом (двоичном) режиме
ввода/вывода, что и требуется в нашем случае.

Рис. 11.12. Питание сервомашинки напрямую от аккумулятора
(если максимальное напряжение аккумулятора не превышает 6 вольт)

Управление сервомашинкой
Для работы с сервомашинками в Arduino IDE имеется отдельная библиотека, при
помощи которой контроллер может генерировать программный ШИМ на любом
порту (кроме A6 и A7) с частотой 50 Гц. Принцип работы контроллера в режиме
генерации ШИМ с частотой 50 Гц очень похож на рассмотренный в главе 5, но для
генерации ШИМ не требуются порты с аппаратной широтно-импульсной модуляцией, — подойдет любой цифровой порт, т. к. генерация ШИМ будет производиться программно. От длины положительного фронта импульса при широтноимпульсной модуляции зависит угол поворота сервомотора. Простейшая программа, реализующая поворот вала сервомотора в разные стороны, представлена в листинге 11.4. Параллельно эта программа поможет нам точно установить рычажок

188

Глава 11

с сонаром на вал сервомашинки (под нужным углом). Вал вначале поворачивается
в среднее положение и остается в нем 5 секунд — эта пауза требуется для точной
установки рычажка с сонаром при монтаже головы, затем поворачивается рывками
по 10 градусов сначала в одну, а затем в другую сторону. Задействуются значения
углов поворота от 10 до 170 градусов (большие или меньшие значения на недорогих сервомашинках могут привести к заклиниванию).
Берегите шестерни сервомоторов!
Сервомашинки SG90 имеют довольно хрупкие шестерни. Не следует с усилием вращать вал руками — это может привести к разрушению шестерней! Моторы с металлическими редукторами таких недостатков лишены.
Листинг 11.4. Программа поворота сервомашинки для робота в разные стороны
// Подключаем библиотеку управления сервомоторами.
#include <Servo.h>
// Создаем сервомотор поворота сонара.
Servo neck;
void setup()
{
// Инициализируем сервомотор, управление GPIO 14.
neck.attach(14);
// 5 секунд на установку головы в центральное положение.
neck.write(90); // Середина шкалы - 90 градусов.
delay(5000);
}
void loop()
{
int i;
// Создаем цикл для равномерного поворота сервомотора.
for (i = 10; i <= 170; i = i + 10)
{
// Поворачиваем вал на угол i.
neck.write(i);
// Приостанавливаем программу.
delay(100);
}
// Возвращаем голову назад.
for (i = 170; i >= 10; i = i - 10)
{
// Поворачиваем вал на угол i.
neck.write(i);
// Приостанавливаем программу.
delay(100);
}
}

Измерение расстояния и поворотная голова

189

Монтаж головы
Пойдем по простому пути и приклеим сервомашинку к корпусу клеем (двусторонний скотч также подойдет). Ультразвуковой сонар также приклеим к рычажку, который идет в комплекте с сервомашинкой, термоклеем, как показано на рис. 11.13.
Важно, чтобы термоклей не попал на микросхемы, иначе возможна деформация их
паяных соединений от высокой температуры, поэтому термоклей нанесем с фронтальной части сонара.

Рис. 11.13. Монтаж сонара на рычажок сервомашинки

Перед установкой рычажка на сервомашинку запустим на роботе программу из
листинга 11.3, но не будем ждать отработки всего кода, а отключим после трех
секунд работы — в это время вал находится в среднем положении. Затем установим
(наденем) рычажок с сонаром так, чтобы сонар смотрел точно вперед. Теперь снова
включим робота (рис. 11.14) и проверим, как вращается сонар, и убедимся, что
проводам от сонара ничего не мешает, — в противном случае провода нужно применить более длинные или изменить положение сервомашинки.

Рис. 11.14. Мобильный робот с поворотным сонаром на сервомашинке

190

Глава 11

Тестовая программа «Ведение цели»
Современные приборы автоматического ведения объекта могут на автомате держать объект в «прицеле». Давайте постараемся реализовать это. Мы имеем: сервомашинку, которая может поворачивать голову робота на заданный угол от 0 до
180 градусов (это в теории, на практике же от примерно 5 до 175 градусов, иначе
возможны заедания механики в крайних положениях), робота с установленным гироприбором MPU-6050, способным отслеживать угол поворота. Если мы отследим
угол поворота робота и компенсируем его поворотом головы, то получим схожий
результат. Отметим, что там, где у робота 0 градусов (начальное положение), у сервомашинки — 90 градусов (серединное положение вала). Не забываем также про
вращение вала со значениями от 10 до 170 градусов.
Расчет угла поворота головы производится следующим образом. В переменной Yaw
содержится угол поворота самого робота: он может изменяться в пределах от –180
до +180 градусов. Предположим, что голова старается удержать нулевое значение
поворота. Ограничим угол поворота пределом в 80 градусов вправо и влево, а затем
к тому, что получили при расчете поворота самого робота в переменной Yaw, прибавим 90 (центральное значение для сервомашинки):
hicken_neck_angle = constrain(Yaw,-80,80)+90

Полная программа представлена в листинге 11.5 — в ней есть неиспользуемые
параметры: инициализированы ходовые моторы и сонар, но в функции loop мы моторы не включаем и расстояние до препятствия не измеряем. Оставлено это сознательно, чтобы вам было проще доработать эту программу под свои идеи. Мы их
используем позднее.
Листинг 11.5. Программа поворота сервомашинки робота в разные стороны
// Подключаем библиотеку, управляющую дальномером.
#include <mobrob3sonar.h>
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
#include <Servo.h>
// Создаем сервомотор поворота головы.
Servo neck;
# define GPIO_chicken_neck A0

Измерение расстояния и поворотная голова

191

// Переменные для хранения поворота по осям.
float Yaw, Pitch, Roll;
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0, delta, _time = 0;
void setup()
{
// Инициализация гироскопа.
MPU6050_DMP_setup();
// Инициализируем моторы.
motor_setup();
// Инициализируем дальномер Trig = 13, Echo = 12.
Sonar_init(13, 12);
// Сервомашинка управляется A0 выводом.
neck.attach(GPIO_chicken_neck);
}
void loop()
{
_millis = millis();
if ((time_out + 11) < _millis)
{
if (YPR_loop()) // Если получены данные от MPU.
{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180 / M_PI;
// Преобразование угла поворота робота
// в угол поворота вала сервомашинки.
int chicken_neck_angle = constrain(Yaw,-80,80)+90;
neck.write(chicken_neck_angle);
time_out = _millis; // Сохраняем время вызова.
}
}
}

Если что-то пошло не так...
Неисправности во время сборки могут возникать по разным причинам: отсутствие
внимания, плохое освещение, неправильное понимание схемы соединений и пр.
Главное — не паниковать, составить схему действий и проверять все потенциально
неисправные узлы последовательно. Даже если что-то сгорит — не стоит расстраиваться, опыт стоит того, а недостающую электронику нетрудно приобрести. После
добавления к роботу новых частей не забывайте до первого включения проверять
отсутствие короткого замыкания по питанию! Для этого измерьте сопротивление
между контактами «плюс» и «минус» питания после выключателя. Момент обнаружения короткого замыкания показан на рис. 11.15. Устраните этот дефект тщательной пропайкой контактов и изоляцией. Также проверьте отсутствие короткого
замыкания на питающих контактах всех вновь установленных приборов.

192

Глава 11

Рис. 11.15. Мультиметр при определении короткого замыкания:
слева — измерение малых сопротивлений; справа — режим прозвона

Выводы
В этой главе подробно рассмотрен процесс установки на робота поворотной головы
на основе сервомашинки с ультразвуковым дальномером HC-SR04/US-026/25, а
также порядок подключения этих приборов.
Теперь робот имеет возможность объезжать препятствия, но пока в нем нет программы, которая анализирует информацию о расстоянии до препятствия и дает
роботу соответствующие команды, — проделанная нами работа была подготовительной. Пора переходить к ходовым испытаниям, чем мы и займемся в следующей
главе.

ГЛАВА 12
ХОДОВЫЕ ИСПЫТАНИЯ:
ОБХОД ПРЕПЯТСТВИЙ

Программа проверки и настройки
основных функций робота
Напишем алгоритм произвольного движения робота, оснащенного поворотным сонаром, с обходом препятствий:
1. Вращаем головой и измеряем расстояние до препятствий слева, справа и впереди, запоминаем полученные значения.
2. Сравниваем расстояния с минимально возможным для продолжения движения
робота и выбираем наибольшее из трех полученных.
3. Если наибольшее расстояние больше минимального, поворачиваем в выбранную
сторону (где наибольшее расстояние) и едем вперед небольшое время.
4. Если наибольшее из измеренных расстояний меньше минимального (не позволяющего продолжить движение) — т. е. тупик, разворачиваемся на 180 градусов.
5. Переход на пункт 1.
Вы должны были заметить, что в большинстве программ, которые мы уже отработали, отсутствует блок задержки: delay(). Дело в том, что при использования гироприбора требуется регулярно делать его опрос и выполнять корректировку по углу.
Но блоки delay() просто останавливают программу — «мозг» робота простаивает,
а ведь может и должен выполнять полезную работу (вычисления, опрос датчиков
и пр.). Поэтому тут необходимо прибегнуть к оптимальному программированию,
которое заключается в применении подхода с выполнением задач по наступлению
определенного момента времени или события. Запускаем действие — например,
поворот головы, но не ждем его окончания, а запоминаем время, когда поворот будет точно завершен (прибавляем к текущему времени 0,5 секунды), — за это время
сервомашинка переместит вал в нужное положение, и только после этого запускаем
измерение расстояния.

194

Глава 12

Введем понятия:
 задания — упорядоченные действия, которые выполняет робот для решения
конкретной задачи (для достижения требуемого результата). Задания можно поделить на периодические, выполняемые через равные промежутки времени,
и ситуативные, выполняемые только при наступлении определенного условия.
Например, задание «Удержание угла поворота робота» выполняется периодически не чаще одного раза в 11 миллисекунд, а время между «выравниваниями» —
свободно и может использоваться для выполнения других заданий;
 поэтапные задания — если задание длительное по времени, и его полное непре-

рывное выполнение приведет к ошибкам в выполнении других заданий, задание
следует разбить на этапы. Например, во время выполнения задания «Удержание
угла поворота робота» нужно периодически проверять отклонение робота от заданного угла. Если при этом робот занят измерениями расстояний и перед началом измерений подруливал, компенсируя ошибку в 1 градус (а робот проводит
измерения расстояний последовательно, каждый раз ожидая поворота головы,
что занимает несколько секунд), то его моторы были включены на поворот,
и все это время он будет вращаться и не только скомпенсирует ошибку, но и
значительно ошибется с поворотом в обратную сторону. В этом случае длительное задание измерения расстояний необходимо разбить на участки — этапы,
между которыми было бы возможно выполнение руления.

Поскольку у нашего робота есть глобальное задание «Движение с обходом препятствий», длительное по времени и периодическое, оно соответственно и разбивается на этапы, а текущий этап определяется значением переменной
global_stage. Этапам даны понятные английские названия: WAIT_SCAN, END_SCAN и
подобные — в программе это константы, которым присвоены оригинальные
числовые значения;
 событие — наступает в результате положительного результата проверки опре-

деленного условия. События приводят к переходу на следующий этап или
выполнению определенных действий внутри этапа. Вот примеры наступления
событий: время выполнения программы больше заданного, угол отклонения
робота от заданного стал меньше 5 градусов, текущий этап WAIT_SCAN, и время,
выделенное на поворот головы истекло (надо начать измерение расстояния);

 состояния global_stage:

• WAIT_SCAN — во время этого этапа идет сканирование слева направо и измеряются три расстояния до препятствия. Когда показания будут сняты, этап
изменится на END_SCAN;
• END_SCAN — сканирование завершено, и производится анализ трех измеренных расстояний до препятствия: Dist_left, Dist_front и Dist_right. Из них
выбирается наибольшее и дополнительно выполняется его проверка на минимально допустимое для движения в эту сторону (нет препятствия). Если
максимальное расстояние больше минимально допустимого, то робот поворачивает в выбранную сторону или сразу движется вперед, если выбирается
Dist_front;

Ходовые испытания: обход препятствий

195

• WAIT_ROTATION — ожидание окончания поворота. Робот не двигается вперед,
пока не завершит поворот на нужный угол CommandYaw;
• END_ROTATION — если робот находился на этапе WAIT_ROTATION, то проверяется
ошибка его поворота и текущая скорость поворота. Если они малы, то робот
переходит на этот этап. Когда этап END_ROTATION наступил, то переменной
PowerMotor, отвечающей за движение, присваивается значение 200 — в результате робот движется вперед с указанной мощностью. После начала движения у робота этап изменяется на WAIT_MOVE;
• WAIT_MOVE — ожидание окончания движения вперед. Во время этого этапа
проверяется, закончено ли отведенное на движение время. Если наступило
такое событие (время 800 миллисекунд истекло), мощность движения обнуляется (PowerMotor=0), и этап изменяется на END_MOVE;
• END_MOVE — на этом этапе роботу дается 200 миллисекунд на торможение.
Цель — полностью завершить движение вперед, которое после окончания
предыдущего этапа еще будет происходить по инерции. По прошествии
200 миллисекунд считается, что робот полностью остановился, и он переходит на этап WAIT_SCAN, во время которого повторяется измерение расстояний
в трех направлениях.
• Во время этапа WAIT_SCAN выполняется ряд подэтапов, при этом переменная
global_stage не изменяется и сохраняет значение WAIT_SCAN, а меняется переменная scan_stage и проверяется время, достаточное для поворота головы
в нужную позицию.
Например, мы измеряем расстояние, после чего даем команду сервомашинке поменять угол поворота. Поворот занимает время около 500 миллисекунд, и следующее
действие по измерению расстояния до препятствия станет возможно сделать, когда
наступит событие «истекло 500 мс». За счет того, что наша программа выполняется
в цикле (когда все команды в loop() заканчиваются, начинается выполнение сначала), мы запоминаем время начала поворота головы в глобальной переменной и периодически, но не постоянно, сравниваем его с текущим временем, ожидая события
«прошло 500 мс». При этом основная программа не останавливается, а выполняется
периодическое задание «Удержание угла поворота».
Программа обхода роботом препятствий, соответствующая приведенному в начале
главы алгоритму и последующим пояснениям, приведена в листинге 12.1.
Листинг 12.1. Программа обхода препятствий роботом
// Подключаем библиотеку, управляющую дальномером.
#include <mobrob3sonar.h>
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>

196

Глава 12

// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
#include <Servo.h>
// Создаем сервомотор поворота головы.
Servo neck;
# define GPIO_chicken_neck A0
// Переменные для хранения поворота по осям.
float Yaw, Pitch, Roll;
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0, delta, delay_time = 0;
float dTime = 0;
float CommandYaw = 0; // Заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
float errorYaw = 0, olderrorYaw = 0;
// Скорость изменения ошибки поворота - дифференциальное звено регулятора.
float dErrorYaw = 0;
// Накопленная ошибка поворота - интегральное звено регулятора.
float iErrorYaw = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotSpeed = 0, RotPower = 0;
int Lmotor = 0, Rmotor = 0, PowerMotor = 0, integRotPower = 0;
// Коэффициенты регулятора для режима удержания угла.
float Kp = 20.0;
float Ki = 15.0;
float Kd = 0.4;
// Переменные для хранения измеренных дистанций.
int Dist_front, Dist_left, Dist_right;
// Этапы работы
#define INACTION 0
// Бездействие.
#define WAIT_SCAN 1
// Ожидание сканирования.
#define END_SCAN 2
// Окончание сканирования.
#define WAIT_ROTATION 3 // Ожидание поворота.
#define END_ROTATION 4 // Окончание поворота.
#define WAIT_MOVE 5
// Ожидание движения.
#define END_MOVE 6
// Окончание движения.
// Минимальная дистанция для проезда.
// Если дистанция меньше MIN_DIST, движение в этом направлении невозможно.
#define MIN_DIST 30
// Переменная для хранения значения одного из четырех.
// Подэтапы внутри этапа WAIT_SCAN.
uint8_t scan_stage = 0; // Возможные значения 0-1-2-3.

Ходовые испытания: обход препятствий
// Переменная для хранения значения текущего этапа работы системы.
uint8_t global_stage = WAIT_SCAN;
// Углы для поворота сонара.
#define left_sonic_angle 160
#define front_sonic_angle 90
#define right_sonic_angle 20
void setup()
{
// Инициализация гироскопа.
MPU6050_DMP_setup();
// Инициализация моторов.
motor_setup();
// Инициализируем дальномер Trig = 13, Echo = 12.
Sonar_init(13, 12);
// Сервомашинка управляется A0 (GPIO_chicken_neck) выводом.
neck.attach(GPIO_chicken_neck);
}
void loop()
{
// Запоминаем текущее время.
_millis = millis();
// Измерение расстояний.
if (global_stage == WAIT_SCAN)
{
if (scan_stage == 0) { // Голову налево.
neck.write(left_sonic_angle);
scan_stage = 1;
delay_time = _millis + 500; // Ждем окончания поворота.
}
if ((scan_stage == 1) && (delay_time < _millis)) {
Dist_left = Sonar(300); // Измеряем слева расстояние.
scan_stage = 2; // Голову прямо.
neck.write(front_sonic_angle);
delay_time = _millis + 500; // Ждем окончания поворота.
}
if ((scan_stage == 2) && (delay_time < _millis)) {
Dist_front = Sonar(300); // Измеряем спереди расстояние.
scan_stage = 3; // Голову направо.
neck.write(right_sonic_angle);
delay_time = _millis + 500;// Ждем окончания поворота.
}
if ((scan_stage == 3) && (delay_time < _millis)) {
Dist_right = Sonar(300); // Измеряем справа расстояние.
scan_stage = 0;
neck.write(front_sonic_angle); // Голову прямо.
global_stage = END_SCAN;
}
}

197

198

Глава 12

// Анализ измеренных расстояний.
if (global_stage == END_SCAN)
{
// Если ехать вперед предпочтительнее - команда ехать прямо.
if ((Dist_front > MIN_DIST) && (Dist_front > Dist_left)
&& (Dist_front > Dist_right)) {
global_stage = END_ROTATION; // Если поворот не нужен.
}
// Если ехать влево предпочтительнее - команда поворот влево.
else if ((Dist_left > MIN_DIST) && (Dist_left > Dist_right)) {
global_stage = WAIT_ROTATION;// Ожидание завершения поворота.
CommandYaw = Yaw - 70;// Команда повернуть налево на 70 градусов.
}
// Если ехать вправо предпочтительнее - команда поворот вправо.
else if (Dist_right > MIN_DIST) {
global_stage = WAIT_ROTATION; // Ожидание завершения поворота.
CommandYaw = Yaw + 70;// Команда повернуть направо на 70 градусов.
}
// Если все измеренные расстояния меньше минимально
// возможного – разворот.
else {
global_stage = WAIT_ROTATION; // Ожидание завершения поворота.
CommandYaw = Yaw + 180; // Развернуться.
}
if (CommandYaw > 180.0) CommandYaw -= 360.0; // Коррекция угла +-180.
if (CommandYaw < -180.0) CommandYaw += 360.0; // Коррекция угла +-180.
}
// Закончили вертеться - начнем ехать.
if (global_stage == END_ROTATION) {
delay_time = _millis + 800; // Мотор будет активен 800 миллисекунд.
PowerMotor = 200; // Наращиваем мощность мотора.
global_stage = WAIT_MOVE; // Устанавливаем статус ожидания движения.
}
// Если время движения истекло.
if ((global_stage == WAIT_MOVE) && (delay_time < _millis)) {
PowerMotor = 0; // Останавливаем движение.
delay_time = _millis + 200; // Даем время на остановку 200 миллисекунд.
global_stage = END_MOVE;
// Устанавливаем статус окончания движения.
}
// Если торможение завершено.
if ((global_stage == END_MOVE) && (delay_time < _millis)) {
global_stage = WAIT_SCAN; // Статус начала сканирования.
}
// Обновляем текущее время.
_millis = millis();
if ((time_out + 11) < _millis)

Ходовые испытания: обход препятствий
{
if (YPR_loop()) // Если получены данные от MPU.
{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180 / M_PI;
// delta - время между вызовами в миллисекундах.
delta = _millis - time_out;
delta = constrain(delta, 10, 70); // Ограничим диапазон.
// dTime - время между вызовами (из миллисекунд в секундах).
dTime = float(delta) * 0.001;
time_out = _millis; // Сохраняем время вызова.
// Расчет заданного угла поворота.
// Требуется стабилизировать только курс - используем Yaw.
// CommandYaw - заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
errorYaw = CommandYaw - Yaw;
if (errorYaw > 180.0) errorYaw -= 360.0; // Коррекция ошибки.
if (errorYaw < -180.0) errorYaw += 360.0; // Коррекция ошибки.
// Скорость изменения ошибки поворота - дифференциальное
// звено регулятора.
dErrorYaw = (errorYaw - olderrorYaw) / dTime;
olderrorYaw = errorYaw;// Запоминаем на будущее
// Накопленная ошибка поворота - интегральное звено регулятора.
// Ki - коэффициент пропорциональности для iErrorYaw.
iErrorYaw = iErrorYaw + errorYaw * dTime * Ki;
// Ограничим интегральную составляющую.
iErrorYaw = constrain(iErrorYaw, -100.0, 100.0);
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
RotPower = errorYaw * Kp + dErrorYaw * Kd + iErrorYaw;
// Если мы на месте: ошибка меньше 0.3 градуса,
// и скорость поворота <0.5 градусов/сек,
// и текущий статус - ожидание поворота.
if ((fabs(errorYaw) < 2.0) && (fabs(dErrorYaw) < 0.5)
&& (global_stage == WAIT_ROTATION))
global_stage = END_ROTATION; // Ставим статус - поворот завершен.
// Ограничим мощность реальным максимумом.
integRotPower = constrain(int(RotPower), -255, 255);
// Расчет мощности, выдаваемой на моторы.
Lmotor = PowerMotor + integRotPower;
Rmotor = PowerMotor - integRotPower;
// Ограничиваем расчет максимально возможными значениями.
Lmotor = constrain(Lmotor, -255, 255);
Rmotor = constrain(Rmotor, -255, 255);
// Команда подать мощность на моторы.
motors_power(Lmotor, Rmotor);
}
}
}

199

200

Глава 12

Задания для самостоятельного решения:
 измените программу так, чтобы робот двигался вперед на меньшее расстоя-

ние, — какие варианты возможны? (Сейчас робот движется 800 миллисекунд
с мощностью моторов 200 из 255);

 как оптимизировать программу, чтобы голова робота уже была в позиции

(левый поворот) по наступлении состояния WAIT_SCAN?
 измените порядок измерения расстояний справа налево;

 вставьте состояние «отдых» (INACTION) перед началом WAIT_SCAN — пусть оно

длится 1000 миллисекунд.

Выводы
Нами создана программа, позволяющая роботу постоянно (в цикле) двигаться,
объезжая препятствия. Для работы робота активно используется гироприбор — по
нему мы отслеживаем точность поворотов.
В следующей главе мы реализуем обход роботом лабиринта, что является схожей,
но более интересной задачей.

ГЛАВА 13
РОБОТ, НАХОДЯЩИЙ
ВЫХОД ИЗ ЛАБИРИНТА

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

Рис. 13.1. Робот в лабиринте

202

Глава 13

В поисках выхода робот не видит лабиринт целиком и не имеет о нем дополнительной информации.
Для достижения поставленной цели нам потребуется решить следующие задачи:
1. Разобраться, как можно установить наличие препятствий (с помощью каких датчиков и решений).
2. С учетом выбранных датчиков и принятых технических решений создать алгоритм движения, гарантирующий прохождение лабиринта.
3. На основании созданного алгоритма разработать программу, позволяющую
роботу преодолеть лабиринт.
4. Подготовить робота, провести его тестирование и добиться стабильной работы.

Способ обхода лабиринта
Известен принцип, согласно которому, если при движении в лабиринте придерживаться одной его стороны (стенки): левой или правой — то выход обязательно
будет достигнут. Это верно для лабиринтов с выходом наружу. Графически возможный путь робота показан на рис. 13.2 (вдоль правой стенки лабиринта) и 13.3
(вдоль его левой стенки).

Рис. 13.2. Путь робота вдоль правой стенки

Робот, находящий выход из лабиринта

203

Рис. 13.3. Путь робота вдоль левой стенки

Следует обратить внимание, что по пути, показанному на рис. 13.3, робот достигнет выхода быстрее, но он не может знать заранее, где выход и какой путь короче.
Конечно, это может знать оператор и выбрать для робота выигрышный режим «направо» или «налево» перед входом в лабиринт. Впрочем, как правило, в соревнованиях роботов такая подсказка оператора недопустима.

Обход лабиринта без модернизации робота
На самом деле небольшая модернизация потребуется, а именно — нужно переставить голову (изменить возможные углы поворота сервомашинки относительно
робота).
Напомним, что в главе 11 были реализованы значения углов поворота от 10 до
170 градусов — робот вращал головой влево/вправо. В лабиринте роботу достаточно смотреть только прямо и вправо, поэтому диапазон углов разумно сместить
вправо так, чтобы он составлял, к примеру, 30–200 градусов, 40–210 градусов или
еще правее.
Изменим положение сонара на валу сервомашинки (снимем и поставим на вал заново) таким образом, чтобы поворот вала позволял проводить измерения спереди
±25 градусов от точного переднего направления и справа ±20 градусов от точного
правого направления. Для настройки углов поворота головы робота предлагается
использовать простую программу (листинг 13.1). Она предусматривает, что после

204

Глава 13

включения сервомашинка должна повернуть голову строго вперед и ждать 5 секунд, затем строго направо и ждать 5 секунд, затем на 20 градусов влево от правого
направления, затем на 20 градусов правее от строго правого. Если положения головы робота не совпадают с заданными по условиям, следует постараться это исправить, сняв рычажок с сервомашинки и установив вновь с поправкой. Если есть
небольшие отклонения, которые физически не исправляются, нужно немного скорректировать параметры front_sonic_angle, right_sonic_angleF и right_sonic_angleR
и добиться нужного эффекта.
Листинг 13.1. Программа настройки углов поворота сервомашинки
// Подключаем библиотеку управления сервомоторами.
#include <Servo.h>
// Создаем сервомотор поворота головы.
Servo neck;
#define front_sonic_angle 130
#define right_sonic_angleF 55
#define right_sonic_angleR 15
void setup()
{
// Инициализируем сервомотор, управление GPIO 14
neck.attach(14);
// 5 секунд на установку головы в центальное положение
neck.write(front_sonic_angle);
delay(5000);
}
void loop()
{
// Поворачиваем голову перпендикулярно стене.
neck.write((right_sonic_angleF + right_sonic_angleR) / 2);
delay(5000);
// Поворачиваем голову в переднее правое положение.
neck.write(right_sonic_angleF);
delay(2000);
// Поворачиваем голову в заднее правое положение.
neck.write(right_sonic_angleR);
delay(2000);
}

При этом спереди будем делать несколько измерений (через 5 градусов), чтобы не
пропустить препятствие и искать наименьшее, а справа — только 2 крайних измерения. При пересчете расстояний учитываем углы поворота от нормали для коррекции реального расстояния (умножаем на косинус угла к нормали).
Если при измерении расстояний спереди от робота (рис. 13.4) были выявлены близкие препятствия, это однозначный поворот налево — если мы попали в подобную
ситуацию, значит, возможности повернуть направо не было. А вот измерение рас-

205

Робот, находящий выход из лабиринта

стояния справа изучим подробнее, обратившись для этого к рис. 13.5, где показаны
варианты измерения пары расстояний, из
которых можно сделать вывод о дальнейших
действиях робота.

Рис. 13.4. Измерения дистанций до препятствия по фронту робота

а

б

в

г

Рис. 13.5. Варианты расположения робота относительно боковой стены

206

Глава 13

 Вариант а — робот движется параллельно правой стене: расстояние в переднем

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

 Вариант б — измеренное расстояние в заднем правом положении больше, чем

в переднем правом положении.

 Вариант в — измеренное расстояние в заднем правом положении меньше, чем

в переднем правом положении.

В обоих этих случаях (б и в) робот должен вернуть себе параллель со стеной, но
с учетом ошибки расстояния до стены.
 Вариант г — робот достиг крутого поворота и должен удачно вписаться в по-

ворот.

Составим алгоритм обхода лабиринта для робота, оснащенного уже знакомым нам
ультразвуковым датчиком, установленным на его поворотной голове (рис. 13.6).

Рис. 13.6. Алгоритм движения вдоль правой стенки с помощью ультразвукового датчика

Робот, находящий выход из лабиринта

207

Пусть робот будет совершать обход лабиринта по правой его стороне. При этом он
должен всегда держаться на небольшом заданном расстоянии от правой стенки и
адекватно реагировать на изменение обстановки. Вначале робот измеряет расстояния во всех требуемых направлениях (в нескольких передних и в двух правых).
Если впереди есть препятствие, то робот не может двигаться вперед — он долженповернуть на месте налево на 60 градусов. Если препятствия нет, то анализируется
состояние измерений вдоль правой стены и на основании среднего расстояния и
ошибки параллельности рассчитывается поворот. По завершении поворота робот
движется вперед непродолжительное время (примерно 300 миллисекунд), затем все
повторяется сначала.

Программа
Алгоритм достаточно прост, но программа (листинг 13.2) выглядит немного сложной, поскольку мы должны периодически опрашивать гироприбор и управлять моторами.
Примечание
При программировании в мультизадачной среде — например, для некоторых контроллеров в Arduino IDE доступна операционная система FreeRTOS, задача обработки
гироприбора и команды колесам выделяется как отдельная. Она запускается однажды
и выполняется в фоне. Тогда программа обхода лабиринта выглядит проще, т. к. в ней
не приходится предусматривать сложные моменты с организацией очереди выполнения операций.

Определенные события в программе выполняются только тогда, когда пришла их
очередь (за это отвечает переменная system_status) и когда время события наступило (за это отвечают переменные delay_time и time_out). Для опроса датчиков
также требуется значительное время, большая часть которого может уходить впустую на ожидание перемещения сервомотора в следующее состояние, однако мы на
это время передаем управление основной программе.
Листинг 13.2. Программа движения вдоль правой стенки
с помощью ультразвукового датчика на сервомашинке
// Подключаем библиотеку, управляющую дальномером.
#include <mobrob3sonar.h>
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5

208

Глава 13

// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
#include <Servo.h>
// Создаем сервомотор поворота головы.
Servo neck;
# define GPIO_chicken_neck A0
// Переменные для хранения поворота по осям.
float Yaw, Pitch, Roll;
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0, delta, delay_time = 0;
float dTime = 0;
float CommandYaw = 0; // Заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
float errorYaw = 0, olderrorYaw = 0;
// Скорость изменения ошибки поворота – дифференциальное
// звено регулятора.
float dErrorYaw = 0;
// Накопленная ошибка поворота - интегральное звено регулятора.
float iErrorYaw = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotSpeed = 0, RotPower = 0;
int Lmotor = 0, Rmotor = 0, PowerMotor = 0, integRotPower = 0, newPowerMotor = 0;
// Коэффициенты регулятора для режима удержания угла.
float Kp = 10.0; // Пропорциональное звено.
float Ki = 15.0; // Интегрирующее звено.
float Kd = 0.4; // Дифференцирующее звено.
#define MIN_DIST 15 // Минимальная дистанция для проезда.
#define MAX_DIST 40 // Мaксимальная измеряемая дистанция.
#define normalRIGHT_DIST 12 // Минимальная дистанция для проезда.
#define Time_move 190 // Время на движение.
// Переменные для хранения измеренных расстояний.
int Dist_front = MAX_DIST, Dist_left = MAX_DIST,
Dist_rightF = MAX_DIST, Dist_rightR = MAX_DIST;
#define WAIT_SCAN 0
// Ожидание сканирования.
#define END_SCAN 1
// Окончание сканирования.
#define WAIT_ROTATION 2 // Ожидание поворота.
#define END_ROTATION 3 // Окончание поворота.
#define WAIT_MOVE 4
// Ожидание движения.
// Этап работы системы.
uint8_t global_stage = 0;
// Опорные углы для поворта сонара.
//#define left_sonic_angle 160
#define front_sonic_angle 130
#define right_sonic_angleF 55
#define right_sonic_angleR 15

Робот, находящий выход из лабиринта

209

// Коэффициент для перевода градусов в радианы.
const float gr_to_rad = (PI / 180.0);
// Коэффициент поправки для расстояний, измеренных с правой стороны.
const float K_cos_in_right =
cos(float((right_sonic_angleF-right_sonic_angleR)/2)* gr_to_rad);
// Функция множественного сканирования УЗ-датчиком.
// Функция работает пошагово, каждый следующий вызов
// делает некоторые действия, а затем,
// если требуется ожидание, завершается,
// но в следующий вызов начинает работу с места останова.
bool scan_all(unsigned long tecTime);
void setup()
{
// Инициализация гироскопа.
MPU6050_DMP_setup();
// Инициализируем моторы.
motor_setup();
// Инициализируем дальномер Trig = 13, Echo = 12.
Sonar_init(13, 12);
// Сервомашинка управляется A0 выводом.
neck.attach(GPIO_chicken_neck);
}
void loop()
{
// Запоминаем текущее время.
_millis = millis();
// Измерение расстояний.
{
// Если состояние ожидания сканирования и время настало.
if ((global_stage == WAIT_SCAN) && (delay_time < _millis))
{
//
if (scan_all(_millis)) global_stage = END_SCAN;
}
// Если сканирование завершено.
if ((global_stage == END_SCAN) ) {
// Новый статус - ожидание поворота.
global_stage = WAIT_ROTATION;
// Если спереди нет препятствия.
if (Dist_front > MIN_DIST) {
newPowerMotor = 255;
// Расчет угла поворота.
// Упрощенная ошибка параллельности стене - робот не паралеллен!
int Err_povorot = (Dist_rightF - Dist_rightR);
// Упрощенная ощибка дистанции от стены // дистанция отличается от заданной ближе/дальше.

210

Глава 13
int Err_distancy = (Dist_rightF + Dist_rightR) / 2 - normalRIGHT_DIST;
CommandYaw = float(Err_povorot + Err_distancy) + Yaw;
}
else {
// Если спереди есть препятствие.
CommandYaw -= 65; // Левый поворот.
newPowerMotor = 255;
}
if (CommandYaw > 180.0) CommandYaw -= 360.0; // Коррекция
// угла +-180.
if (CommandYaw < -180.0) CommandYaw += 360.0; // Коррекция
// угла +-180.
PowerMotor = 0;
}
// Если поворот завершен.
if (global_stage == END_ROTATION) {
// Новый статус: ожидаем завершения движения.
global_stage = WAIT_MOVE;
// Подаем мощность на моторы.
PowerMotor = newPowerMotor;
// Выделяем время на продвижение робота вперед.
delay_time = _millis + Time_move;
}
// Если время, выделенное на движение, истекло.
if ((global_stage == WAIT_MOVE) && (delay_time < _millis) ) {
// Новый этап - начало сканирования.
global_stage = WAIT_SCAN;
// Обнуляем мощность на движение вперед.
PowerMotor = 0;
// Дадим роботу время на остановку
delay_time = _millis + 200; // Ждем окончания поворота.
}

}
// Обновляем текущее время.
_millis = millis();
if ((time_out + 11) < _millis)
{
if (YPR_loop()) // Если получены данные от MPU.
{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180 / M_PI;
// delta - время между вызовами в миллисекундах.
delta = _millis - time_out;
delta = constrain(delta, 10, 70); // Ограничим диапазон.
// dTime - время между вызовами (из миллисекунд в секундах)
dTime = float(delta) * 0.001;
time_out = _millis; // Сохраняем время вызова.

Робот, находящий выход из лабиринта
// Расчет заданного угла поворота.
// Требуется стабилизировать только курс - используем Yaw.
// CommandYaw - заданный угол поворота.
// Ошибка поворота - пропорциональное звено регулятора.
errorYaw = CommandYaw - Yaw;
if (errorYaw > 180.0) errorYaw -= 360.0; // Коррекция ошибки.
if (errorYaw < -180.0) errorYaw += 360.0; // Коррекция ошибки.
// Скорость изменения ошибки поворота - дифференциальное
// звено регулятора
dErrorYaw = (errorYaw - olderrorYaw) / dTime;
olderrorYaw = errorYaw; // Запоминаем на будущее
// Накопленная ошибка поворота - интегральное звено регулятора.
// Ki - коэффициент пропорциональности для iErrorYaw.
iErrorYaw = iErrorYaw + errorYaw * dTime * Ki;
// Ограничим интегральную составляющую.
iErrorYaw = constrain(iErrorYaw, -100.0, 100.0);
// Мощность, передаваемая на моторы, для компенсации
// ошибки поворота
RotPower = errorYaw * Kp + dErrorYaw * Kd + iErrorYaw;
// Если мы на месте: ошибка меньше 0.3 градуса
// и скорость поворота <0.5 градусов/сек
// и текущий статус - ожидание поворота.
if ((fabs(errorYaw) < 2.0) && (fabs(dErrorYaw) < 0.5) &&
(global_stage == WAIT_ROTATION))
global_stage = END_ROTATION; // Ставим этап - поворот завершен.
// Ограничим мощность реальным максимумом.
integRotPower = constrain(int(RotPower), -255, 255);
// Расчет мощности, выдаваемой на моторы.
Lmotor = PowerMotor + integRotPower;
Rmotor = PowerMotor - integRotPower;
// Ограничиваем расчет максимально возможными значениями.
Lmotor = constrain(Lmotor, -255, 255);
Rmotor = constrain(Rmotor, -255, 255);
// Команда подать мощность на моторы.
motors_power(Lmotor, Rmotor);
}
}
}
//
//
//
//
//
//
//
//

Функция пошагового сканирования.
Каждый вызов включает в текущий статус (фазу) действий
анализ текущего времени на окончание периода ожидания...
Выполнение действия с изменением состояния, измерение расстояния,
передачу управления основной программе
Когда все измерения завершены, функция возвращает значение true это сигнал для основной программы, что можно действовать дальше,
поскольку измерения завершены.

211

212
bool scan_all(unsigned long tecTime)
{
// Статическая - неуничтожаемая переменная
static uint8_t scan_stage = 0;
// Статическая - неуничтожаемая переменная
// для действия
static unsigned long reacTime = 0;
// Статическая - неуничтожаемая переменная
static int tec_angle = front_sonic_angle +

Глава 13

этапа работы.
времени пробуждения

с углом поворота сонара.
25;

if (scan_stage == 3)// Начало нового сканирования.
{
// Сбрасываем минимальное значение спереди.
Dist_front = MAX_DIST;
scan_stage = 0;
}
// Блок множественного сканирование спереди.
if ((scan_stage == 0) && (reacTime <= tecTime))
{
// Измеряем расстояние.
float Dist_tec = float(Sonar(MAX_DIST));
// Учитываем и корректируем расстояние исходя из угла
// поворота головы.
Dist_tec=cos(fabs(float(tec_angle-front_sonic_angle)*gr_to_rad))*Dist_tec;
// Ищем минимальное расстояние.
Dist_front = min(Dist_front, int(Dist_tec));
tec_angle -= 5;
reacTime = tecTime + 200;
if (tec_angle < front_sonic_angle - 25)
{
tec_angle = right_sonic_angleF;
reacTime += 300;
scan_stage = 1;
}
neck.write(tec_angle);
}
// Блок граничного сканирования справа (1-я часть).
if ((scan_stage == 1) && (reacTime <= tecTime))
{
// Измеряем расстояние с учетом угла поворота.
Dist_rightF = int(float(Sonar(MAX_DIST))*K_cos_in_right);
reacTime = tecTime + 300;
tec_angle = right_sonic_angleR;
scan_stage = 2;
neck.write(tec_angle);
}

Робот, находящий выход из лабиринта

213

// Блок граничного сканирования справа (2-я часть).
if ((scan_stage == 2) && (reacTime <= tecTime))
{
// Измеряем расстояние с учетом угла поворота.
Dist_rightR = int(float(Sonar(MAX_DIST))*K_cos_in_right);
reacTime = tecTime + 500;
tec_angle = front_sonic_angle + 25;
scan_stage = 3;
neck.write(tec_angle);
return true;
}
return false;
}

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

ГЛАВА 14
РОБОТ С ВИДЕОКАМЕРОЙ

Мы уже умеем свободно управлять роботом, заставлять его выполнять определенные действия, он также научился автономно делать обход по черной линии и искать выход из лабиринта. А что если нас нет рядом с ним, но требуется визуально
рассмотреть обстановку вокруг робота или приказать роботу объехать некоторое
помещение и осмотреть его? Предлагаем установить на нашего робота камеру
с возможностью транслировать видеоизображение по беспроводному интерфейсу.

Аналоговое видео с робота
Существует масса вариантов, как это сделать.
Например, на робота можно установить аналоговую камеру (такие используются
в FPV-системах квадрокоптеров) и аналоговый видеопередатчик VTX. На рис. 14.1
показана подобная камера со встроенным видеопередатчиком. Камеру нужно запитать от отдельного стабилизатора на 3,3–5 вольт, но существуют и варианты со
встроенным стабилизатором, которые можно подключать к более высокому напряжению.
Тогда на нашей стороне понадобится экран с подключенным приемником аналогового видео, который будет принимать передачу в режиме реального времени
(рис. 14.2). Приемников аналогового видеосигнала также масса вариантов: от небольшого модуля без экрана, который подключается к смартфону (рис. 14.3), до
качественных видеоочков с отдельными экранами для каждого глаза и подстройкой
межзрачкового расстояния и диоптрий.
Подобные системы (рис. 14.4) способны передавать и принимать видео с дальностью до нескольких километров, а при использовании мощного передатчика и качественных антенн — и десятков километров, но преимущественно в пределах
прямой видимости. Деревья, дома, холмы сильно ослабляют принимаемый сигнал,
но внутри квартиры или в поле на дальности до 1 километра робот будет все видеть
хорошо. Останется вопрос: как управлять самим роботом, ведь соединения Bluetooth или Wi-Fi, способы управления по которым мы рассматривали в предшест-

215

Робот с видеокамерой

вующих главах, имеют дальность сигнала 10–50 метров? В этом случае и систему
управления нужно переводить на использование «дальнобойного» оборудования:
пульт дистанционного управления типа показанного на рис. 8.1 и специальный
приемник команд на роботе — например, flySky-i6B.

Рис. 14.1. Аналоговая камера со встроенным
видеопередатчиком по радиоканалу

Рис. 14.2. Видеоприемник, состоящий
из дисплея и приемника аналогового
видеосигнала

Рис. 14.3. Видеоприемник, состоящий из отдельного приемника сигнала,
подключенного к смартфону

216

Глава 14

Передача видеоизображения
на частоте 5,8 ГГц

Рис. 14.4. Приемопередача аналогового видео от камеры со встроенным видеопередатчиком
к дисплею, оборудованному VTX-приемником

Цифровое видео для робота: ESP32-CAM
Конечно, существуют системы, способные передавать цифровой видеосигнал на
дальнее расстояние — например, знаменитые дроны DJI справляются с этой задачей при дальности в десятки километров. Здесь же мы рассмотрим один из самых
доступных вариантов с использованием готового модуля с камерой и интерфейсом
Wi-Fi — модуль ESP32-CAM.
Для нашего робота следует выбрать модуль ESP32-CAM с широкоугольным объективом (рис. 14.5), что позволит получить достаточный угол обзора для удобного
управления. Преимущество этого варианта еще и в том, что ESP32-CAM можно
программировать из среды ArduinoIDE, о чем будет рассказано далее. Помимо камеры и контроллера ESP32, на модуле установлен мощный светодиод, который
можно дистанционно включать и освещать им пространство перед камерой.

217

Робот с видеокамерой

а

б

Рис. 14.5. Камера ESP32-CAM с установленным широкоугольным объективом:
а — вид сверху; б — вид сбоку

Внешние выводы платы модуля ESP32-CAM показаны на рис. 14.6, они также продублированы для возможности работы с внутренним картридером (чтобы сохранять фото на карту памяти).

а

б

Рис. 14.6. а — плата модуля ESP32-CAM; б — габариты платы и внешние выводы

218

Глава 14

На ESP32-CAM смонтирована встроенная антенна Wi-Fi, но для увеличения дальности прима/передачи есть возможность установить внешнюю антенну в предусмотренный для нее IPEX-разъем. Но просто вставить внешнюю антенну недостаточно — требуется еще переустановить (перепаять) перемычку, которая по умолчанию включена на работу с внутренней антенной (рис. 14.7).

Рис. 14.7. Выбор антенны

Робот с камерой ESP32-CAM
Рассмотрим проект, в котором на сервомашинку нашего робота вместо ультразвукового сонара будет установлен модуль ESP32-CAM.
ESP32-CAM мы прошьем скетчем, который создаст на основе ESP32-CAM точку
удаленного доступа Wi-Fi. При подключении смартфона к этой точке доступа
по адресу 192.168.4.1 в браузере отроется сайт, на странице которого будет транслироваться видео с камеры и присутствовать несколько кнопок для управления
роботом.
При нажатии пользователем кнопок на странице сайта модуль ESP32-CAM будет
по Serial-порту передавать роботу команды на повороты и движения, которые
робот сможет читать и выполнять.
В отличие от варианта с аналоговой камерой, это решение более трудоемкое, но и
значительно более дешевое.
Помимо модуля ESP32-CAM, нам понадобится адаптер, который позволит залить
в модуль прошивку. Рекомендутся использовать специальный адаптер именно для
ESP32-CAM — он называется ESP32-CAM-MB (рис. 14.8, слева). Модуль ESP32CAM устанавливается в этот адаптер (рис. 14.8, справа), после чего мы можем программировать его почти как стандартный Arduino-контроллер.

Робот с видеокамерой

219

Рис. 14.8. Адаптер ESP32-CAM-MB (слева); модуль ESP32-CAM установлен а адаптер (справа)

Добавление контроллеров ESP в среду ArduinoIDE
Перед прошивкой скетча нужно подготовить среду Arduino IDE, поскольку по
умолчанию контроллеры фирмы ESP в ней не поддерживаются.
Итак, запускаем среду Arduino IDE и выбираем пункт меню Файл | Настройки
(рис. 14.9). Здесь нас интересует пункт Дополнительные ссылки для Менеджера

Рис. 14.9. Окно настроек Arduino IDE

220

Глава 14

плат. Нажав кнопку справа, попадаем в окно ввода ссылок (рис. 14.10) и добавляем
ссылку на ESP32: https://raw.githubusercontent.com/espressif/arduino-esp32/ghpages/package_esp32_index.json — эта ссылка подключает дополнительный репозиторий, созданный программистами Espressif и содержащий основные компоненты для работы с ESP32 в среде Arduino IDE.

Рис. 14.10. Окно ввода дополнительных ссылок для Менеджера плат

Закрываем окно кнопкой ОК, проверяем в окне настроек наличие введенной строки
и подтверждаем внесенные изменения, закрыв окно Настройки нажатием кнопки ОК.
Теперь надо установить в Arduino IDE библиотеки и компилятор для ESP32. Для
этого переходим в меню Инструменты | Плата, в открывшемся меню выбора плат
выбираем Менеджер плат и в строке поиска Менеджера плат вводим ESP32. Если
предварительные настройки были сделаны правильно, получаем в окне ссылку на
скачивание модулей поддержки ESP32 ( рис. 14.11). Нажимаем кнопку Установка

Рис. 14.11. Пакет ESP32 найден — можно приступать к его установке

Робот с видеокамерой

221

и ждем завершения процесса скачивания недостающих файлов с сайта компании
Espressif.

Прошивка ESP32-CAM
Для программирования ESP32-CAM выберем в среде ArduinoIDE нужную плату
для прошивки (рис. 14.12) и зальем в нее представленный в листинге 14.1 скетч.
В скетче можно изменить имя точки доступа и пароль к точке доступа. Модуль
ESP32-CAM при этом должен быть установлен в ESP32-CAM-MB, ESP32-CAMMB подключен к компьютеру, и в среде ArduinoIDE выбран порт для прошивки.
Непосредственно в процессе прошивки (после того как в среде Arduino нажата
кнопка Загрузка) нажмите на ESP32-CAM-MB кнопку IO0 — это переведет модуль
ESP32-CAM в режим программирования. Кнопку эту нужно удерживать до окончания прошивки.

Рис 14.12. Выбор платы AI Thinker ESP32-CAM

Листинг 14.1. Прошивка для управления роботом от ESP32-CAM
// Имя точки доступа.
const char* ssid = "Mobrobot";
// Пароль для подключения к точке доступа.
const char* password = "123456789";
#include <mobrob3espcam.h>
void setup() {
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector
// Включить порт обязательно, иначе не будет связи с
// основным роботом.
Serial.begin(9600);

222

Глава 14

// Инициализация камеры.
mob_camera_setup();
// Старт Web-сервера.
startCameraServer();
}
void loop() {
}

Настройка программного обеспечения робота
За основу программного обеспечения для нашего робота (листинг 14.2) возьмем
листинги 8.4 и 13.1. Из листинга 8.4 выберем фрагмент, обеспечивающий реакцию
на приходящие по Serial-соединению символы и организацию ответа моторами,
а из 13.1 — управление сервомашинкой.
С ESP32-CAM будут приходить только символы:
 F — движение вперед;
 B — движение назад;
 L — поворот влево;
 R — поворот вправо;
 l — поворот головы с камерой влево;
 r — поворот головы с камерой вправо;
 c — поворот головы с камерой в центр;
 S — остановка робота.

Нам остается принять эти символы и выполнить определенное действие исходя из
их назначения.
Листинг 14.2. Программа для робота для связи с ESP32-CAM
// Переменная для приема данных по Bluetooth.
char bt_input;
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Подключаем библиотеку управления сервомоторами.
#include <Servo.h>

Робот с видеокамерой
// Создаем сервомотор поворота головы.
Servo neck;
#define front_servo_angle 90
#define left_servo_angle 170
#define right_servo_angle 10
// Инициализация.
void setup()
{
// Устанавливаем скорость передачи данных по Bluetooth.
Serial.begin(9600);
// Инициализируем моторы.
motor_setup();
// Инициализируем сервомотор, управление GPIO 14
neck.attach(14);
// 5 секунд на установку головы в центральное положение.
neck.write(front_servo_angle);
}
// Время на движение. Если в течение этого времени
// новых команд не будет, робот остановится.
const unsigned long move_time = 5000;
unsigned long _time = 0;
// Мощность на моторы до 255.
int Power=150;
void loop()
{
// Если пришли данные по Bluetooth.
if (Serial.available())
{
// Записываем, что пришло по Bluetooth, в переменную bt_input.
bt_input = (char)Serial.read();
_time = millis();
switch (bt_input) {
// Вперед.
case 'F':
motors_power(Power, Power);
break;
// Назад.
case 'B':
motors_power(-Power, -Power);
break;
// Влево.
case 'L':
motors_power(-Power, Power);
break;
// Вправо.
case 'R':

223

224

Глава 14
motors_power(Power, -Power);
break;
// Стоп.
case 'S':
motors_power(0, 0);
break;
// Камеру прямо.
case 'c':
neck.write(front_servo_angle);
break;
// Камеру влево.
case 'l':
neck.write(left_servo_angle);
break;
// Влево.
case 'r':
neck.write(right_servo_angle);
break;
}
_time = millis();

}
if ((millis() - _time) > move_time)

motors_power(0, 0);

}

Сборка электроники
Робот и камера теперь прошиты и содержат требуемые для успешной работы программы, но нужно связать их физически: добавить стабилизатор для питания
ESP32-CAM и соединить Serial-порты обоих модулей. На рис. 14.13 показано подключение ESP32-CAM к электропитанию через отдельный стабилизатор на 5 вольт.
Контакт U0R ESP32-CAM соединен с TX0 ArduinoUNO, а контакт U0T ESP32CAM соединен с RX0 ArduinoUNO — таким образом они смогут общаться друг
с другом, передавать сообщения (в нашем случае ESP32-CAM передает команды
в виде символов).

Сборка механики
Камеру ESP32-CAM нужно прикрепить к рычажку сервомашинки — например,
термоклеем, как показано на рис. 14.14, и установить рычажок так, чтобы при выставлении команды на переднее положение камеры камера смотрела вперед.
При проведении тестовых заездов (рис. 14.15) робот с подобной камерой и внешней
антенной транслировал видеосигнал даже через кирпичную стену. Но подобные
препятствия могут приводить к тому, что реакция робота на ваши команды замедлится и видео будет редко обновляться, кроме того, некоторые команды могут не

Робот с видеокамерой

Рис. 14.13. Подключение ESP32-CAM к питанию робота и соединение Serial-портов

Рис. 14.14. Робот с камерой ESP32-CAM

225

226

Глава 14

Рис. 14.15. Изображение с камеры робота на экране смартфона

прийти, и робот «подвиснет». После повторного подключения обязательно нужно
закрыть страницу в браузере и заново открыть ее, набрав 192.148.4.1 в адресной
строке.
Теперь постарайтесь самостоятельно добавить роботу функцию удержания угла по
гироприбору и вообще «прокачайте» его по максимуму.

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

ГЛАВА 15
РОБОТ
И ПРОЙДЕННОЕ РАССТОЯНИЕ

Мы научились пользоваться гироприбором, удерживать угол поворота, но довольно
простую вещь — пройденное расстояние — измерить роботом напрямую не можем. Кроме того, мы до сих пор не контролируем скорость движения: скорость поворота контролируем, а скорость движения — нет. Конечно, существуют приборы,
которые помогут нам в этом вопросе, их масса — например, для роботов, которые
движутся на открытой местности и на большие расстояния, можно использовать
данные со спутников (GPS, GLONASS), но для комнатного робота возможно более
простое решение — подсчет оборотов колес робота. И одно из самых простых
устройств, способных реализовать такое решение, — инкрементальный энкодер.

Моторы с инкрементальным энкодером
Простейшие моторчики, которые можно было бы использовать для установки энкодеров, подходят слабо. Есть и иные решения, но они сопряжены с переделкой
самих моторов, что требует некоторого профессионализма, поэтому расмотрим моторы, которые уже имеют энкодер и понижающий редуктор. Так, вполне подойдут
китайские редукторные моторы JGA25-370 на 6 вольт с энкодером и редуктором
(маркировка энкодера: 170 или 280 об/мин).
Для мотора с инкрементальным энкодером (рис. 15.1)
требуется 6 отдельных проводов: 2 — силовых на
щетки мотора, 2 — на питание энкодера, 2 — для
показаний энкодера.

Рис. 15.1. Мотор с редуктором
и инкрементальным энкодером

228

Глава 15

Как работает инкрементальный энкодер?
На валу мотора с тыльной его части надет круглый магнит с диаметральной намагниченностью — т. е. магнитное поле по кругу отличается (рис. 15.2). Снаружи от
магнита также по кругу под углом друг к другу установлены два датчика Холла,
которые способны реагировать на изменение магнитного поля. Когда вал мотора
вращается, изменяется и направление магнитного поля, и это изменение и улавливают датчики Холла, которые на своем выходе устанавливаются в состояние 0 или 1 (высокий или
низкий потенциал).
Силовые контакты моторов с энкодерами подключаются так же, как контакты моторов из предыдущих глав (рис. 15.3), но для наших конкретных
моторов JGA25-370 силовые контакты (согласно
документации) — это красный и белый провода. Рис. 15.2. Пример диаметрально
намагниченного круглого магнита
Если провода перепутать, то можно испортить датчики Холла!

Рис. 15.3. Подключение силовых контактов моторов с энкодерами

Робот и пройденное расстояние

229

Подключение контактов энкодеров показано на рис. 15.4: питание энкодеров подключим к 5 вольт контроллера Arduino (синий провод — к +5V, черный провод —
к GND), а сигнальные контакты от датчиков Холла (желтый и зеленый провода) —
от левого мотора подключим к 11 и 10 GPIO, а от правого — к 9 и 8 GPIO.

Рис. 15.4. Подключение контактов энкодеров

Получение информации о вращении
с инкрементальных энкодеров
Пара датчиков в энкодере расположена так, что их состояние изменяется не одновременно, а поочередно. Если вал мотора равномерно вращать в одну сторону, то
показания датчиков будут изменяться так, как показано на рис. 15.5.
Это позволяет довольно просто контролировать вращение, учитывая порядок изменения состояния. Нужно лишь знать предыдущее состояние, и если оно изменится,

230

Глава 15

Рис. 15.5. График изменения состояния пары датчиков в энкодере

то мы либо прибавляем к счетчику единицу, либо отнимаем ее, следуя правилу из
рисунка.
Программа работы с инкрементальными энкодерами прямо из функции Loop()
приведена в листинге 15.1. Следует учесть, что если выполнять подобные расчеты
прямо в программе, которая производит какие-либо длительные действия, можно
пропустить время, когда контроллер занят, и проскочить одно или несколько состояний, что приведет к ошибке. Поэтому подобный расчет в реальных проектах
производится в фоновом режиме «из прерывания».
Листинг 15.1. Пример работы с инкрементальными энкодерами прямо из основного
цикла программы (только если программа ничего больше не делает)
// GPIO энкодеров.
// Левое колесо.
#define enLEFT_1 11
#define enLEFT_2 10
// Правое колесо.
#define enRIGHT_1 9
#define enRIGHT_2 8
// Переменные для чтения энкодеров.
bool L1 = 0, L2 = 0, R1 = 0, R2 = 0;
// Для анализа положения энкодеров.
int Eleft = 0, Eright = 0, EleftOld = 0, ErightOld = 0;
// Счетчики энкодеров.
long encoder_R = 0, encoder_L = 0,
encoder_Rold = 0, encoder_Lold = 0;
// Инициализация.
void setup()
{
pinMode(enLEFT_1, INPUT);
pinMode(enLEFT_2, INPUT);
pinMode(enRIGHT_1, INPUT);
pinMode(enRIGHT_2, INPUT);

Робот и пройденное расстояние
// Считываем начальное положение энкодеров.
L1 = digitalRead(enLEFT_1);
L2 = digitalRead(enLEFT_2);
R1 = digitalRead(enRIGHT_1);
R2 = digitalRead(enRIGHT_2);
// Расчет аналитических переменных.
Eleft = (L2 << 1) | L1;
EleftOld = Eleft;
R1 = digitalRead(enRIGHT_1);
R2 = digitalRead(enRIGHT_2);
Eright = (R2 << 1) | R1;
ErightOld = Eright;
}
void loop()
{
// Считываем данные с энкодеров и формируем из них переменные.
L1 = digitalRead(enLEFT_1);
L2 = digitalRead(enLEFT_2);
Eleft = (L2 << 1) | L1;
R1 = digitalRead(enRIGHT_1);
R2 = digitalRead(enRIGHT_2);
Eright = (R2 << 1) | R1;
// Если зафиксировано изменение положения левого энкодера.
if (Eleft != EleftOld)
{
// Анализируем направление вращения.
switch (EleftOld)
{
case 2:
{
if (Eleft == 3) encoder_L++;
if (Eleft == 0) encoder_L--;
break;
}
case 0:
{
if (Eleft == 2) encoder_L++;
if (Eleft == 1) encoder_L--;
break;
}
case 1:
{
if (Eleft == 0) encoder_L++;
if (Eleft == 3) encoder_L--;
break;
}

231

232

Глава 15
case 3:
{
if (Eleft == 1) encoder_L++;
if (Eleft == 2) encoder_L--;
break;
}
}
EleftOld = Eleft;

}
// Если зафиксировано изменение положения правого энкодера.
if (Eright != ErightOld)
{
// Анализируем направление вращения.
switch (ErightOld)
{
case 2:
{
if (Eright == 3) encoder_R++;
if (Eright == 0) encoder_R--;
break;
}
case 0:
{
if (Eright == 2) encoder_R++;
if (Eright == 1) encoder_R--;
break;
}
case 1:
{
if (Eright == 0) encoder_R++;
if (Eright == 3) encoder_R--;
break;
}
case 3:
{
if (Eright == 1) encoder_R++;
if (Eright == 2) encoder_R--;
break;
}
}
ErightOld = Eright;
}
// Если были вращения колес, вывести новые значения в порт.
if ((encoder_R != encoder_Rold) || (encoder_L != encoder_Lold))
{
encoder_Lold = encoder_L; encoder_Rold = encoder_R;
Serial.print(encoder_L); Serial.print(" "); Serial.println(encoder_R);
}
}

233

Робот и пройденное расстояние

Будем считать, что вы раздобыли нужные моторы и комплекты деталей крепления
их к колесам установили на платформу своего робота (рис. 15.6 и 15.7). Давайте
создадим программу, которая умеет регулировать скорость движения робота, основываясь на показаниях энкодеров.

а

б

в
Рис. 15.6. Сборка пары из мотор-энкодеров с фабричными колесами:
а — комплект необходимых деталей; б — схема сборки; в — колесная пара в сборе

Рис. 15.7. Робот с мотор-энкодерами вид снизу (слева) и сверху (справа)

Программа в листинге 15.2 получает состояние энкодеров в фоновом режиме (по
прерыванию). Один раз в 40 микросекунд срабатывает прерывание и проверяется
состояние энкодеров: если состояние энкодеров изменилось, то производится ана-

234

Глава 15

лиз по алгоритму из листинга 15.1 и счетчик вращения энкодера увеличивается или
уменьшается на 1 (в зависимости от направления вращения).
Мы спрятали от вас в библиотеке mobrob3encoder.h организацию вызова прерывания
и весь расчет из листинга 15.1, немного его оптимизировав. За основу взято управление роботом из RemoteXY — чтобы можно было следить за текущим состоянием
энкодеров.
Подключитесь к роботу со смартфона с помощью приложения RemoteXY (настройки по умолчанию такие же, как и для HC-06, — см. разд. «Добавляем функцию движения» главы 9) и джойстиком управляйте моторами. Если при движении
вперед один из энкодеров считает не в ту сторону, измените true на false для соответствующего энкодера:
#define leftWELLrot true
#define righrWELLrot true

перепрошейте контроллер и заново запустите программу. Добейтесь того, чтобы
при движении вперед показания обоих энкодеров росли. Тест состояния энкодеров
приведен в листинге 15.2. Установки leftWELLrot и righrWELLrot потребуются
в следующей программе (листинг 15.3).
Листинг 15.2. Тест состояния энкодеров из интерфейса RemoteXY
(управление по умолчанию — через HC-06)
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>
// Определение режима соединения и подключение библиотеки RemoteXY.
// #define REMOTEXY_MODE__ESP8266_HARDSERIAL_POINT //ESP01
#define REMOTEXY_MODE__HARDSERIAL // HC06
#include <RemoteXY.h>
// Настройки соединения.
#define REMOTEXY_SERIAL Serial // HC06
#define REMOTEXY_SERIAL_SPEED 9600 // HC06
#define REMOTEXY_ACCESS_PASSWORD "1234" // HC06
//#define REMOTEXY_SERIAL_SPEED 115200 //ESP01
//#define REMOTEXY_WIFI_SSID "RemoteXY" //ESP01
//#define REMOTEXY_WIFI_PASSWORD "12345678" //ESP01
//#define REMOTEXY_SERVER_PORT 6377 //ESP01
// Конфигурация интерфейса.
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
// 27 bytes

Робот и пройденное расстояние
{ 255, 2, 0, 51, 0, 21, 0, 16, 31, 1, 5, 32, 5, 43, 53, 53, 2, 26, 31, 67,
4, 5, 5, 52, 7, 2, 26, 51 };
// Структура определяет все переменные и события
// вашего интерфейса управления.
struct {
// input variables
int8_t joystick_1_x; // oт -100 до 100
int8_t joystick_1_y; // oт -100 до 100
// output variables
char text_1[51]; // =строка UTF8, оканчивающаяся нулем
// other variable
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
/////////////////////////////////////////////
// GPIO энкодеров - эти данные нельзя изменять, адреса жестко привязаны
// данные только для информации.
// Чтобы использовать другие GPIO,
// нужно внести изменения в файл mobrob3pid.h.
// Левое колесо.
#define enLEFT_1 11
#define enLEFT_2 10
// Правое колесо.
#define enRIGHT_1 9
#define enRIGHT_2 8
// Если направление в энкодере считается неверно,
// т. е. при движении вперед - энкодер уменьшается,
// измените для такого колеса true на false.
#define leftWELLrot true
#define righrWELLrot true
// Переменные для хранения данных от энкодеров.
long LeftEncoder = 0; long RightEncoder = 0;
#include <mobrob3pid.h>
// Переменные времени.
unsigned long _millis = 0, time_out = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotSpeed = 0, RotPower = 0;
int Lmotor = 0, Rmotor = 0, PowerMotor = 0, integRotPower = 0;
//====================
void setup()
{
RemoteXY_Init ();
// Инициализируем энкодеры.
encoder_setup();
// Инициализируем моторы.
motor_setup();
}

235

236

Глава 15

//====================
void loop()
{
RemoteXY_Handler ();
_millis = millis();
// Если время настало для опроса MPU.
if ((time_out + 200) < _millis)
{
LeftEncoder = readLeftEncoder();
RightEncoder = readRightEncoder();
String TempString = "Le=" + String(LeftEncoder) +
" Re=" + String(RightEncoder);
TempString.toCharArray(RemoteXY.text_1, 50);
// Расчет мощности моторов по показаниям джойстика.
PowerMotor = RemoteXY.joystick_1_y;
Lmotor = PowerMotor + RemoteXY.joystick_1_x;
Rmotor = PowerMotor - RemoteXY.joystick_1_x;
// Передача на моторы мощности.
motors_power(Lmotor, Rmotor);
// Запомнить время вызова.
time_out = _millis;
}
}

Следим за энкодерами в приложении
В качестве устройства удаленной связи здесь используется модуль ESP01 на скорости связи 115 200 бод. Модуль ESP01 подключен к отдельному стабилизатору на
3,3 вольта (рис. 15.8), что улучшило качество его работы. Кроме того, раздельное
питание ESP01 и Arduino позволяет при программировании Arduino оставлять
ESP01 без питания — тогда он не мешает прошивке, хотя и подключен к порту
Serial.
Следующая программа (листинг 15.3) будет не только удерживать курс робота, как
мы уже делали, но и производить перемещение на заданное расстояние. Управление осуществляется джойстиком, а на экране смартфона отображается текущий
угол поворота и позиция по энкодерам с учетом знака (рис. 15.9).
В программе применены четыре регулятора, но их реализация — для простоты —
спрятана в библиотечный файл mobrob3pid.h:
1. Регулятор угла поворота — имеет три звена (линейное, интегрирующее и дифференцирующее). Коэффициент пропорциональности для каждого из них соответственно зависит от ошибки угла (rKp = 10.0), накопленной во времени ошибки угла (rKi = 15.0), и скорости изменения ошибки угла (rKd = 0.4).
Для вызова функции расчета нужно знать: Yaw — текущий угол робота,
угол, к которому стремится повернуть робот, и dTime — время от

CommandYaw —

Робот и пройденное расстояние

Рис. 15.8. Схема подключения ESP01 с внешним стабилизатором

Рис. 15.9. Интерфейс управления

237

238

Глава 15

последнего вызова этой функции. На выходе (CommandRotSpeed) — рекомендуемая скорость поворота робота в разности скорости колес:
CommandRotSpeed = PID_Angle_rotangle_calc(Yaw, CommandYaw, dTime)

2. Регулятор линейного перемещения — имеет три звена (линейное, интегрирующее и дифференцирующее). Коэффициент пропорциональности для каждого из
них соответственно зависит от ошибки линейного перемещения (mKp = 5.0),
накопленной во времени ошибки линейного перемещения (mKi = 0) и скорости
изменения ошибки линейного перемещения (mKd = 0.01).
Для вызова функции расчета нужно знать: move_counter — текущее состояние
перемещения по энкодерам, CommandSteps — требуемое перемещение и dTime —
время от последнего вызова этой функции. На выходе (CommandSpeed) — рекомендуемая линейная скорость робота:
CommandSpeed = PID_move_calc(float(move_counter), CommandSteps,dTime)

3 и 4. Регуляторы скорости колеса — имеют по два звена (линейное и дифференцирующее). Коэффициент пропорциональности для каждого из них соответственно зависит от ошибки линейной скорости (smKp = 0.01) и скорости изменения ошибки линейной скорости (smKd = 0.005).
Для каждого мотора предусмотрены два регулятора — это связано с тем, что
в регуляторе должны храниться данные расчета из предыдущего вызова.
Для вызова функции расчета нужно знать: RealSpeedL/RealSpeedR — текущую
скорость колеса по энкодеру, CommandSpeedL/CommandSpeedR — заданную скорость
для колеса, Lmotor/Rmotor — мощность, поданнyю в настоящий момент на
моторы, dTime — время от последнего вызова этой функции. На выходе
(Lmotor/Rmotor) — обновленная мощность, поданная на моторы робота:
Lmotor=PID_moveSpeed_calcLEFT(RealSpeedL, CommandSpeedL, Lmotor, dTime)
Rmotor=PID_moveSpeed_calcRIGHT(RealSpeedR, CommandSpeedR, Rmotor, dTime)
Листинг 15.3. Программа с регулированием перемещения
(управление по умолчанию — через ESP01 на скорости 115200)
// i2c адрес MPU6050. Может быть 0x68 или 0x69.
#define MPU6050ADR 0x68
// Подключаем библиотеку для MPU6050.
#include <MPU6050dmp.h>
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Подключаем библиотеку управления моторами.
#include <mobrob3xmotor.h>

Робот и пройденное расстояние
// Определение режима соединения и подключение библиотеки RemoteXY.
#define REMOTEXY_MODE__ESP8266_HARDSERIAL_POINT //ESP01
//#define REMOTEXY_MODE__HARDSERIAL // HC06
#include <RemoteXY.h>
// Настройки соединения.
#define REMOTEXY_SERIAL Serial
//#define REMOTEXY_SERIAL_SPEED 9600 // HC06
//#define REMOTEXY_ACCESS_PASSWORD "1234" // HC06
#define REMOTEXY_SERIAL_SPEED 115200 //ESP01
#define REMOTEXY_WIFI_SSID "RemoteXY" //ESP01
#define REMOTEXY_WIFI_PASSWORD "12345678" //ESP01
#define REMOTEXY_SERVER_PORT 6377 //ESP01
// Конфигурация интерфейса.
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =
// 60 bytes
{ 255, 2, 0, 15, 0, 53, 0, 16, 31, 1, 5, 32, 12, 57, 39, 39, 2, 26, 31, 67,
4, 5, 3, 52, 7, 2, 26, 11, 71, 56, 8, 10, 47, 47, 0, 2, 24, 255, 0, 0,
52, 195, 0, 0, 52, 67, 0, 0, 240, 65, 0, 0, 32, 65, 0, 0, 0, 64, 24, 0
};
// Структура определяет все переменные и события
// вашего интерфейса управления
struct {
// input variables
int8_t joystick_1_x; // oт -100 до 100
int8_t joystick_1_y; // oт -100 до 100
// output variables
char text_1[11]; // =строка UTF8, оканчивающаяся нулем.
float instrument_1; // oт -180 до 180
// other variable
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
/////////////////////////////////////////////
// Переменные для хранения поворота по осям.
float Yaw, OldYaw = 0, Pitch, Roll, CommandYaw = 0;
// Хранение данных времени.
unsigned long time_out = 0, _millis = 0, delta, _time = 0;
float dTime = 0;
// Мощность, передаваемая на моторы для компенсации ошибки поворота.
float RotSpeed = 0;
int Lmotor = 0, Rmotor = 0, PowerMotor = 0;
#define MAXROTSPEED 1000.0 // тиков энкодера в секунду (не градусов).
#define MAXSPEED 2000.0
// тиков энкодера в секунду.
#define MAXMOVEPOWER 200
// макс. 255 для Arduino UNO ШИМ.
#define MAXPOWER 255 // Максимальная мощность на моторы.
// GPIO энкодеров. Эти данные нельзя изменять, адреса жестко привязаны
// и даны только для информации.

239

240
// Чтобы использовать другие GPIO,
// нужно внести изменения в файл mobrob3pid библиотеки.
// Левое колесо.
#define enLEFT_1 11
#define enLEFT_2 10
// Правое колесо.
#define enRIGHT_1 9
#define enRIGHT_2 8
// Если направление в энкодере считается не верно,
// т. е. при движении вперед энкодер уменьшается, // измените для этого колеса true на false.
#define leftWELLrot true
#define righrWELLrot false
// Переменные для хранения данных от энкодеров.
long LeftEncoder = 0, OldLeftEncoder = 0, RightEncoder = 0,
OldRightEncoder = 0 ;
long CommandSteps = 0;
// Счетчики перемещения.
long Oldmove_counter = 0, move_counter = 0;
// Скорости, расчитанные по энкодерам.
float RealSpeed = 0 , RealSpeedL, RealSpeedR;
// Скорости поворота (угловые).
float RealRotSpeed = 0;
float CommandRotSpeed = 0;
// Заданные скорости – желаемые.
float CommandSpeed = 0, CommandSpeedL = 0, CommandSpeedR = 0;
// Библиотечный модуль содержит работу с энкодерами и ПИД-регуляторы.
#include "mobrob3pid.h"
void setup()
{
// Активация удаленного управления.
RemoteXY_Init ();
// Коэффициенты ПИД-регулятора для режима удержания угла.
// Расчет скорости поворота в тиках энкодера.
rKp = 40.0; rKi = 145.0; rKd = 0.1;
// Коэффициенты ПИД-регулятора для регуляции скорости перемещения.
mKp = 5.0; mKi = 0; mKd = 0.01;
// Коэффициенты ПИД-регулятора для режима удержания скорости
// вращения колеса.
smKp = 0.01; smKd = 0.005;
// Инициализируем энкодеры.
encoder_setup();
// Инициализируем моторы.
motor_setup();
// Инициализация гироскопа.
MPU6050_DMP_setup();
}

Глава 15

Робот и пройденное расстояние

241

void loop()
{
_millis = millis();
// Если время настало для опроса MPU.
if (time_out + 50 <= _millis)
{
if (YPR_loop()) // Если получены данные от MPU-гироприбора.
{
// Преобразуем углы в градусы из радиан.
Yaw = ypr[0] * 180.0 / M_PI;
// delta - время между вызовами в миллисекундах.
delta = _millis - time_out;
// dTime - время между вызовами (в секундах).
dTime = float(delta) * 0.001;
time_out = _millis; // Сохраняем время вызова.
// Управление моторами, если подключен смартфон.
if (RemoteXY.connect_flag == 1)
{
// Расчет скорости поворота в тиках энкодера на колесо.
CommandRotSpeed = PID_Angle_rotangle_calc(Yaw, CommandYaw, dTime);
// Снимаем показания с энкодеров.
LeftEncoder = readLeftEncoder();
RightEncoder = readRightEncoder();
// Расчет текущего "среднего" перемещения.
move_counter = (LeftEncoder + RightEncoder) / 2;
// Расчет скорости перемещения, требуемой для достижения
// поставленной позиции.
// Скорость движения линейная.
// Расчет скорости вращения для левого колеса.
CommandSpeed = PID_move_calc(float(move_counter), CommandSteps, dTime);
CommandSpeedL = CommandSpeed + CommandRotSpeed;
CommandSpeedL = constrain(CommandSpeedL, -MAXSPEED, MAXSPEED);
// Расчет скорости вращения для правого колеса.
CommandSpeedR = CommandSpeed - CommandRotSpeed;
CommandSpeedR = constrain(CommandSpeedR, -MAXSPEED, MAXSPEED);
// Нахождение текущей скорости левого колеса.
RealSpeedL = (LeftEncoder - OldLeftEncoder) / dTime;
// Расчет ПИД-регулятора подаваемой мощности ШИМ на левый мотор.
Lmotor = PID_moveSpeed_calcLEFT(RealSpeedL, CommandSpeedL, Lmotor,
dTime);
// Нахождение текущей скорости левого колеса.
RealSpeedR = (RightEncoder - OldRightEncoder) / dTime;
// Расчет ПИД-регулятора подаваемой мощности ШИМ на правый мотор.
Rmotor = PID_moveSpeed_calcRIGHT(RealSpeedR, CommandSpeedR, Rmotor,
dTime);
// Подача мощности на моторы.
motors_power(Lmotor, Rmotor);

242

Глава 15
// В память текущие значения на энкодерах.
OldLeftEncoder = LeftEncoder;
OldRightEncoder = RightEncoder;
}
OldYaw = Yaw;
}
// Если тайм-аут превышен - отключаем моторы.
if (time_out + 55 < _millis)
{
Lmotor = 0; Rmotor = 0;
motors_power(Lmotor, Rmotor);
}

}
static unsigned long time_in_opros = 0;
if (time_in_opros < _millis)
{
RemoteXY_Handler ();
static uint8_t i = 0;
if (i > 10) // Ограничим вывод значений 1 раз в 100 миллисекунд.
{
if (RemoteXY.connect_flag == 1)// Удаленное управление подключено.
{
// В поле экрана помещаем значение перемещения.
String TempString;
TempString = String(move_counter);
TempString.toCharArray(RemoteXY.text_1, 10);
// В круговой стрелочный индикатор - значение текущего
// угла поворота.
RemoteXY.instrument_1 = Yaw;
}
i = 0;
}
i++;
if (RemoteXY.connect_flag == 1) // Удаленное управление подключено.
{
// На какую позицию нужно переехать (вперед-назад).
CommandSteps = CommandSteps + long(RemoteXY.joystick_1_y / 2);
// Какого угла поворота нужно придерживаться.
CommandYaw = CommandYaw + float(RemoteXY.joystick_1_x) * 0.03;
// Угол должен лежать в рамках +-180 градусов.
if (CommandYaw > 180.0) CommandYaw -= 360.0; // Коррекция ошибки
if (CommandYaw < -180.0) CommandYaw += 360.0; // Коррекция ошибки
}
else
motors_power(0, 0); // Если соединение потеряно - выключить моторы.
time_in_opros = _millis + 10; // Следующий вызов через 10 миллисекунд.
}
}

Робот и пройденное расстояние

243

Самостоятельно модифицируйте код из листинга 15.3, чтобы робот мог проходить
некоторую записанную траекторию. Программа должна состоять из следующих
условий: если сделали предыдущую команду, начать выполнять следующую, никаких задержек выполнения программы delay() быть не должно. Для примера на
рис. 15.10 и 15.11 приведены варианты траекторий, соответствующих соревновательной дисциплине кегельринг.

Рис. 15.10. Кегельринг — траектория
для выбивания всех кеглей

Рис. 15.11. Кегельринг — траектория
для выбивания кеглей с постоянным возвратом
в центр

Выводы
Мы научились считывать информацию с энкодеров, преобразовывать ее в перемещение робота, использовать перемещение для расчета текущей скорости и регулировать скорость робота.

ГЛАВА 16
БАЛАНСИРУЮЩИЙ РОБОТ

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

Теория
Балансирующий робот представляет собой обратный маятник. Это похоже на тот
случай, когда вы ставите на ладонь или палец длинную палочку и стараетесь компенсировать ее падение, перемещая руку в ту сторону, в которую палочка начинает
падать. Но компенсировать падение палочки сложнее — ведь робот все-таки имеет
два колеса и не падает вправо и влево, а только вперед и назад. На рис. 16.2 слева
изображен сбалансированный робот. Если робот выведен из состояния равновесия,
сила тяжести уже не направлена строго по центру и начинает действовать на робота
так, что приводит его к падению. Чем больше наклон робота, тем составляющая
силы тяжести, направленная в сторону падения робота, выше (на рис. 16.2 — это
СТД, сила тяжести действующая). Для компенсации силы тяжести мы будем использовать силу инерции, направленную в противоположную сторону от СТД.
Обозначим ее СИД — сила инерции действующая). Нужную силу инерции можно
создать, если двигаться с положительным ускорением в сторону падения (стрелка

Балансирующий робот

Рис. 16.1. Балансирующий робот на моторах с энкодерами

Рис. 16.2. Обратный маятник в равновесном состоянии (слева)
и в состоянии наклона с попыткой его компенсации

245

246

Глава 16

Направление движения). При этом важно, чтобы СИД не только компенсировала
СТД, но и была немного больше, что приведет к выправлению робота.

Как найти угол наклона робота?
Углы отклонения объекта от вертикали рассчитываются по данным акселерометра,
который регистрирует ускорение свободного падения, направленное к центру Земли. На рис. 16.3 изображен неподвижный объект (робот) на наклонной плоскости.
Координатные оси акселерометра привязаны к роботу. Ось Y направлена вперед по
движению, ось Z — вниз перпендикулярно движению, ось X — к наблюдателю.
⎛a ⎞
AngYZ = arctan ⎜ Y ⎟ ,
(16.1)
⎝ aZ ⎠
где AngYZ — угол отклонения робота от вертикали в плоскости, формируемой осями Y и Z; aY — проекция ускорения свободного падения на ось Y акселерометра;
aZ — проекция на ось Z.
Можно также использовать формулы:
⎛a ⎞
⎛a ⎞
AngYZ = arcsin ⎜ Y ⎟ или AngYZ = arccos ⎜ Z ⎟ ,
⎝ g ⎠
⎝ g ⎠
где g — ускорение свободного падения.
Для электронного прибора, который будет рассмотрен далее, рекомендуется использовать формулу (16.1), оперирующую только относительными величинами.

AngYZ
Рис. 16.3. Нахождение угла отклонения от вертикали (робот неподвижен)

247

Балансирующий робот

На практике акселерометр довольно сильно «шумит», и используя только его, мы
получим не точный угол наклона, а существенные колебания значений около искомого угла.
Поэтому для нахождения отклонения по одной оси мы воспользуемся математически обработанными данными от гироскопа и акселерометра. Самым простым является комплементарный фильтр:
⎛a
Ang komp = ( Ang 0komp + Gy × dT ) × 0.99 + arctan ⎜ Y
⎝ aZ

⎞
⎟ × 0.01 ,
⎠

(16.2)

где Ang komp — рассчитанный угол наклона робота; Ang 0komp — угол наклона из
прошлой итерации (от которой прошло небольшое время dT); Gy — угловая скорость (умножением ее на dT получаем приращение угла); 0.99 — коэффициент
⎛a ⎞
пропорциональности для результата с приращением по гироскопу; arctan ⎜ Y ⎟ —
⎝ aZ ⎠
угол, рассчитанный по данным акселерометра; 0.01 — коэффициент пропорциональности для данных от акселерометра. В сумме два коэффициента пропорциональности должны давать 1.

Регулятор балансировки робота
Обратите внимание, что регулятор должен задавать именно ускорение (изменение
скорости), а не скорость:
Speed = Speed real + Re g ,

Speed — скорость, с которой должен двигаться робот; Speed real — скорость с которой робот двигается в настоящее время (измеряем ее по показаниям энкодеров);
Re g — изменение скорости рассчитаем по следующей формуле:
Re g = ErrA × Kpea + dErrA × Kd ea + ErrS × Kpes + dErrS × Kd es + iErrS × Kies ,

где:
 ErrA — ошибка угла наклона (в нашем случае это угол от комплементарного
фильтра);
 dErrA — скорость изменения ошибки угла наклона (производная от угла накло-

на) «сопротивляется» изменению угла наклона робота, каким бы он ни был;
 ErrS — ошибка линейной скорости робота (если скорость движения для робота

не задана, ошибка равна текущей скорости робота);
 dErrS — скорость изменения ошибки линейной скорости. Гасит колебания от

ErrS ;

 iErrS — интегральная накопленная ошибка скорости робота. Это очень важный

элемент, он компенсирует отклонение нуля гироприбора от реального центра
равновесия робота. Работа без ErrS приводит к колебаниям робота (робота
мотает, как маятник с большой амплитудой);

248

Глава 16

 Kpea , Kd ea , Kpes , Kd es , Kies — коэффициенты пропорциональности звеньев ре-

гулятора. Они подбираются, и как их подобрать, будет рассмотрено на реальном
примере в программе (листинг 16.1).

Программирование
Возможности восьмибитового контроллера Arduino UNO ограниченны, поэтому
для реализации программы балансировки робота следует освободить ресурсы.
В частности, пришлось отказаться от использования управления через RemoteXY,
изменить генерацию ШИМ-управления моторами и добавить HC-06 к роботу, описанному в разд. «Следим за энкодерами в приложении» главы 15 (рис. 16.4). Чтобы
не снимать ESP01, в схему установлен переключатель на три положения, который
подключает питание ESP01 или HC-06 либо отключает оба.
Обратите внимание, что стандартно модуль HC-06 запитывается напряжением
5 вольт через имеющийся на плате понижающий преобразователь. Но может работать и от напряжения 3.3 вольта. Это напряжение следует подать на соответствующий вход питания 3.3 вольта (припаять отдельный провод к боковому контакту
платы HC-06) (см. рис. 16.4).

Рис. 16.4. Обновленная схема подключения модулей удаленного управления

Балансирующий робот

249

Программа балансировки приведена в листинге 16.1. При этом в комплект примеров кода, зашитый в библиотеку mobrob3.zip, включен еще листинг 16.3, который
отличается от листинга 16.1 наличием блока приема команд от HC-06/05 в приложении Bluetooth RC Controller.
Директива #include <mobrob3pidmotor.h> активирует энкодеры и ШИМ-моторов
(encoder_setup()). Она также содержит ПИД-регуляторы моторов (motors_
power(Lmotor, Rmotor)) и опрос энкодеров (LeftEncoder = readLeftEncoder(),
RightEncoder = readRightEncoder()).
Директива #include <MPU6050one_axis_angle.h> активирует гироприбор (setup_
mpu6050()), опрашивает гироприбор и рассчитывает угол наклона (angle_recalc(1 ,
90)). Первое число здесь — выбор направления и оси, второе — смещение нуля,
расчет угла попадает в переменную AcYsum).
Для корректной работы программы требуется установить из стандартных библиотек Arduino IDE библиотеку Adafruit MPU6050 — она используется для организации обмена информацией с MPU6050 функций из модуля <MPU6050one_axis_
angle.h>. Делается это стандартно: из меню Arduino IDE выполните команду
Скетч | Подключить библиотеку | Управлять библиотеками, затем в поисковой
строке наберите Adafruit MPU6050, среди найденных результатов найдите библиотеку Adafruit MPU6050 и произведите ее установку.
Программа в листинге 16.1 снабжена понятными комментариями. Для достижения
положительного эффекта следует скорректировать программу под своего робота,
поэкспериментировав с коэффициентами.
Листинг 16.1. Программа балансировки с использованием комплементарного фильтра
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Не подключаем библиотеку управления моторами !!!!!!
// Моторы инициализируются и управляются из одного прерывания с энкодерами.
// GPIO энкодеров. Эти данные нельзя изменять, адреса жестко привязаны
// и даны только для информации.
// Чтобы использовать другие GPIO,
// нужно внести изменения в файл mobrob3pid библиотеки.
// Левое колесо.
#define enLEFT_1 11
#define enLEFT_2 10
// Правое колесо.
#define enRIGHT_1 9
#define enRIGHT_2 8

250

Глава 16

// Если направление в энкодере считается не верно, т. е. при движении вперед
// энкодер уменьшается, измените для этого колеса true на false.
#define leftWELLrot true
#define righrWELLrot false
// Переменные для хранения данных от энкодеров.
long LeftEncoder = 0, RightEncoder = 0;// OldLeftEncoder = 0,
OldRightEncoder = 0 ;
// Библиотечный модуль содержит работу с энкодерами и ПИД-регуляторы.
#define MAXROTSPEED 1000.0 // Тиков энкодера в секунду (не градусов).
#define MAXMOVESPEED 2000.0 // Макс. скорость управляемого движения.
#define MAXSPEED 5500.0
// Тиков энкодера в секунду.
#define MAXMOVEPOWER 255
// Макс. 255 для Аrduino UNO ШИМ.
#define MAXPOWER 255
// Максимальная мощность на моторы.
// ПИД-регуляторы.
#include <mobrob3pidmotor.h>
// Обслуживание гироприбора.
#include <MPU6050one_axis_angle.h>
uint32_t millis_, Delay_time = 0;
float dTime, ErrorAngle, dErrorAngle, oldErrorAngle = 0;
float RealSpeed, ErrorSpeed, CommandSpeed = 0, dErrorSpeed,
OldErrorSpeed = 0, iErrorSpeed = 0;
// Расчетная скорость.
float Speed;
// Коэффициенты пропорциональности регулятора балансировки робота.
// Kp_ea - к ошибке угла; Kd_ea - к скорости изменения ошибки угла;
// Kp_s - к ошибке скорости перемещения робота;
// Kd_s - к скорости изменения ошибки скорости робота;
// Ki_s - к интегралу от ошибки скорости перемещения робота по времени.
const float Kp_ea = 850.5, Kd_ea = 88.5, Kp_s = -3.5, Kd_s = -0.00025, Ki_s = -7;
// Состояние робота.
int8_t StartBall;
// Типичные состояния робота.
const int8_t preBALANCING_MODE = 1; // Подготовка к балансировке.
const int8_t STOP_MODE = 0;
// Робот лежит.
const int8_t BALANCING_MODE = 2;
// Балансировка.
// Оси и направления расчета угла.
// При падении вперед угол должен расти...
const int8_t YAXISPLUS = 0; // Балансировка.
const int8_t YAXISMINUS = 1; // Балансировка.
const int8_t XAXISPLUS = 2; // Балансировка.
const int8_t XAXISMINUS = 3; // Балансировка.
// Корректировка угла (ноль должен устанавливаться в AcYsum,
// когда робот стоит в положении балансировки...
const float ANGLE_CORRECT = 90;

Балансирующий робот
// Скорости.
float CommandSpeedL = 0, CommandSpeedR = 0, CommandRotSpeed = 0;
// Измеренные скорости.
float RealSpeedL = 0, RealSpeedR = 0;
// Мощность на моторы.
float Lmotor = 0, Rmotor = 0;
// Счетчик состояния подготовки к балансировке.
uint8_t counter_preBAL = 0;
// Массивы предыдущих значений энкодеров по колесам
// хранятся за последние 50 миллисекунд с интервалом 5 миллисекунд.
long OldLeftEncoder[10], OldRightEncoder[10];
//======================
void setup() {
// Serial для вывода контрольных значений на ПК при настройке робота.
Serial.begin(115200);
setup_mpu6050();
// Активация удаленного управления.
// Коэффициенты ПИД-регулятора для режима удержания
// скорости вращения колеса.
// smKp - к ошибке скорости колеса,
// smKd - к производной от ошибки скорости колеса
// (скорость изменения ошибки скорости колеса)
smKp = 0.005; smKd = 0.00025;
// Инициализируем энкодеры, инициализируем моторы.
encoder_setup();
// Запрет балансировки в первые 2 секунды.
Delay_time = millis() + 2000;
// Принудительное обнуление элементов массива.
for (int i = 0; i < 10; i++)
{
OldLeftEncoder[i] = 0;
OldRightEncoder[i] = 0;
}
// Быстрый, но не точный расчет угла – подготовительный.
balancing_preparation(YAXISMINUS , ANGLE_CORRECT);
}
//=============================================
void loop()
{
// Текущее время
millis_ = millis();
// Пересчет наклона по оси балансировки.
bool result = angle_recalc(YAXISMINUS , ANGLE_CORRECT);
// При включении робота комплементарному фильтру требуется
// небольшое время, чтобы прийти в равновесие, после чего
// он будет считать углы наклона верно.

251

252

Глава 16

if (millis_ < Delay_time) return;
if (result == true) // Если рассчитаны очередные показания
// угла наклона робота
{
// Если робот упал, запретим расчеты по балансировке.
if (abs(AcYsum) > 45) StartBall = STOP_MODE;
else if (StartBall == STOP_MODE)
{ // Если мы подняли робота в состояние балансировки.
if (abs(AcYsum) < 5) {
// Активируем подготовительную фазу балансировки.
StartBall = preBALANCING_MODE;
counter_preBAL = 0;
}
}
// Время между опросами в секундах.
dTime = float(millis_ - Delay_time) * 0.001;
// Снимаем показания с энкодеров.
LeftEncoder = readLeftEncoder();
RightEncoder = readRightEncoder();
static int i = 0; // Счетчик для массива предыдущих
// значений с энкодеров
// Нахождение текущей скорости левого колеса.
// Скорость измеряем – расстояние, пройденное колесом за 50 миллисекунд.
// При меньшем времени возрастают погрешности
// за счет дискретности энкодеров.
RealSpeedL = float(LeftEncoder - OldLeftEncoder[i]) / (10.0 * dTime);
// Нахождение текущей скорости правого колеса.
RealSpeedR = float(RightEncoder - OldRightEncoder[i]) / (10.0 * dTime);
// Ошибка угла наклона!
ErrorAngle = AcYsum;
// Реальная средняя скорость.
RealSpeed = (RealSpeedL + RealSpeedR) / (2.0);
// Ошибка скорости движения относительно заданной скорости.
ErrorSpeed = CommandSpeed - RealSpeed;
// Если робот выставлен на балансировку.
if (StartBall > 0)
{
// Скорость изменения ошибки угла наклона.
dErrorAngle = (ErrorAngle - oldErrorAngle) / dTime;
// Скорость изменения ошибки скорости движения (ускорение ошибки).
dErrorSpeed = (ErrorSpeed - OldErrorSpeed) / dTime;
// Интеграл от ошибки скорости по времени (путь ошибки).
iErrorSpeed += ErrorSpeed * dTime * Ki_s;
// Ограничение интеграла ошибки - не обязательно.
iErrorSpeed = constrain(iErrorSpeed, -20000.0, 20000.0);
// Если мы только выставили робота и еще держим его рукой,
// не позволяем роботу
if (StartBall == preBALANCING_MODE) {

Балансирующий робот

253

// В режиме подготовки балансировка слабая.
iErrorSpeed = 0;
ErrorSpeed = 0;
dErrorSpeed = 0;
ErrorAngle = constrain(ErrorAngle , -5.0, 5.0);
dErrorAngle = (ErrorAngle - oldErrorAngle) / dTime;
counter_preBAL++;
// Если время подготовки истекло - пора балансировать
// в полную силу.
if (counter_preBAL > 120 ) StartBall = BALANCING_MODE;
}
// ПИД-регулятор балансировки робота.
// По факту мы создаем ускорение (отрицательное или положительное).
// Составляющие:
// 1.Ошибка угла;
// 2.Скорсть изменения ошибки угла;
// 3.Ошибка скорости;
// 4.Производная от ошибки скорости;
// 5.Интеграл от ошибки скорости.
// За скобками время периода, оно здесь только для того,
// чтобы не менять коэффициенты пропорциональности,
// если мы изменили длительность периода между опросами.
// Расчет суммируется с текущей скоростью,
// создавая отрицательное или положительное ее приращение.
// Робот ускоряется - начинает действовать сила инерции.
Speed = RealSpeed + (ErrorAngle * Kp_ea + dErrorAngle * Kd_ea +
ErrorSpeed * Kp_s + dErrorSpeed * Kd_s + iErrorSpeed) * dTime / 0.05;
if (StartBall != BALANCING_MODE) Speed = constrain(Speed, -200, 200);
// Еще "подмешиваем" скорость поворота робота.
CommandSpeedL = Speed + CommandRotSpeed;
// Ограничиваем максимальную скорость.
CommandSpeedL = constrain(CommandSpeedL, -MAXSPEED, MAXSPEED);
CommandSpeedR = Speed - CommandRotSpeed;
CommandSpeedR = constrain(CommandSpeedR, -MAXSPEED, MAXSPEED);
// Расчет ПИД-регулятора подаваемой мощности ШИМ на левый мотор.
Lmotor=PID_moveSpeed_calcLEFT(RealSpeedL, CommandSpeedL, Lmotor, dTime);
// Расчет ПИД-регулятора подаваемой мощности ШИМ на правый мотор.
Rmotor=PID_moveSpeed_calcRIGHT(RealSpeedR, CommandSpeedR, Rmotor, dTime);
}
else
{
// Закомментируйте, если желаете заблокировать колеса
// робота в сост. покоя.
Lmotor = 0; Rmotor = 0;
// Раскомментируйте, чтобы моторы были заблокированы в сост. покоя //
увеличивает потребление энергии.
//
CommandSpeedL = 0;
//
CommandSpeedR = 0;

254

Глава 16
//

// Расчет ПИД-регулятора подаваемой мощности ШИМ
// на левый мотор
//Lmotor=PID_moveSpeed_calcLEFT(RealSpeedL,CommandSpeedL, Lmotor, dTime);
//
// Расчет ПИД-регулятора подаваемой мощности ШИМ
// на правый мотор
//Rmotor=PID_moveSpeed_calcRIGHT(RealSpeedR,CommandSpeedR,Rmotor, dTime);
ErrorSpeed = 0;
}
// Подача мощности на моторы
motors_power(int(Lmotor), int(Rmotor));
// В память текущие значения на энкодерах.
OldLeftEncoder[i] = LeftEncoder;
OldRightEncoder[i] = RightEncoder;
OldErrorSpeed = ErrorSpeed;
oldErrorAngle = ErrorAngle;
Delay_time = millis_;
i++; if (i > 9) i = 0;
}
{
// Вывод контрольных значений
// может использоваться для коррекции оси наклона и сдвига нуля.
// Также его можно использовать при настройке регулятора моторов.
static unsigned long control_time = 0;
// НЕ ЗАБУДЬТЕ ОТКЛЮЧИТЬ устройство удаленного доступа!
if (control_time < millis_)
{
Serial.print(AcYsum); Serial.print(" "); Serial.print(RealSpeedL);
Serial.print(" "); Serial.print(RealSpeedR); Serial.println();
control_time = millis_ + 200;
}
}
}

Порядок подбора
коэффициентов пропорциональности регулятора
В предыдущей главе мы уже настраивали ПИД-регулятор моторов. В программе,
приведенной в листинге 16.1, его коэффициенты немного изменены. Правильно
начать настройку именно с моторов. Программа настройки моторов заключается
в том, что у моторов меняются заданные скорости и отслеживается поведение моторов. Считаем, что правый и левый моторы похожи, и коэффициенты у них одинаковые. При этом у нас есть два звена, которые управляют изменением мощности,
подаваемой на моторы, в зависимости от требуемой скорости.

Балансирующий робот

255

Итак, для подбора коэффициентов:
1. Загрузите в робота программу из листинга 16.2. Не подключая устройство
внешнего управления (HC-06), подключите питание робота, кабель связи с ПК
не выдергивайте. Из Arduino IDE откройте монитор порта — вы увидите в строке три нулевых значения: первое — заданная скорость, второе — текущая скорость левого колеса, третье — текущая скорость правого колеса.
2. Чтобы изменить скорость моторов на 100 единиц (тиков энкодера), нужно послать в порт символы F (+100) или B (–100). Каждая посланный символ F увеличивает заданную для моторов скорость на 100, B — соответственно на 100
уменьшает. Символ S — резко останавливает моторы.
3. 3. Обнулите smKd (smKd = 0). Задайте небольшое значение: smKp = 0.001, перекомпилируйте программу и снова загрузите ее в робота. Пошлите в порт символы FFFF — заданная скорость увеличится до 400. Визуально вы заметите, что
скорость моторов не постоянна — она гармонически колеблется, это видно и по
измеренным результатам (рис. 16.5).

Рис. 16.5. Результаты работы программы настройки моторов
с разными коэфициентами smKp и smKd регулятора моторов

256

Глава 16

4. Увеличьте smKd (smKd = 0.0001), перекомпилируйте программу, снова загрузите
ее и наблюдайте результаты измерения при скорости 400 тиков. Можно видеть,
что колебания прекратились, но мотор недостаточно быстро перестраивает скорость.
5. Еще увеличьте smKp... Увеличение пропорционального ошибке скорости коэффициента опять приведет к колебаниям (smKp = 0.005, smKd = 0.0001). Если задать слишком большое значение smKd, это тоже приведет к значительным колебаниям (см. на рис. 16.5 при smKd = 0.001).
6. Добейтесь, чтобы моторы адекватно держали заданную скорость, а при изменении заданной скорости переходили на новую в сжатый промежуток времени и
без гармонических колебаний.
Как можно видеть, при smKp = 0.005 и smKd = 0.00025 мотор довольно быстро меняет скорость, и колебаний скорости нет. Остановимся на этом значении.
Настройка ваших моторов может отличаться — все зависит от мощности моторов,
их редукторов и даже массы обода колеса.
Листинг 16.2. Программа подбора коэффициентов регулятора для моторов
// Описание управления моторами.
// Контакты управления левого мотора только 2 и 3.
# define GPIO_1F 2
# define GPIO_1B 3
// Контакты управления правого мотора только 4 и 5.
# define GPIO_2F 4
# define GPIO_2B 5
// Не подключаем библиотеку управления моторами !!!!!!
// Моторы инициализируются и управляются из одного
// прерывания с энкодерами.
// GPIO энкодеров. Эти данные нельзя изменять, адреса жестко привязаны
// и даны только для информации.
// Чтобы использовать другие GPIO,
// нужно внести изменения в файл mobrob3pidmotor.h библиотеки.
// Левое колесо
#define enLEFT_1 11
#define enLEFT_2 10
// Правое колесо
#define enRIGHT_1 9
#define enRIGHT_2 8
// Если направление в энкодере считается не верно,
// т.е. при движении вперед энкодер уменьшается,
// измените для этого колеса true на false.
#define leftWELLrot true
#define righrWELLrot false
// Переменные для хранения данных от энкодеров.
long LeftEncoder = 0, RightEncoder = 0;

Балансирующий робот
// Библиотечный модуль содержит работу с энкодерами и ПИД-регуляторы.
#define MAXROTSPEED 1000.0 // Тиков энкодера в секунду (не градусов).
#define MAXMOVESPEED 2000.0 // Макс. скорость управляемого движения.
#define MAXSPEED 5500.0 // Тиков энкодера в секунду.
#define MAXMOVEPOWER 255 // Макс. 255 для Arduino UNO ШИМ.
#define MAXPOWER 255 // Максимальная мощность на моторы.
// ПИД-регуляторы.
#include <mobrob3pidmotor.h>
uint32_t millis_, Delay_time = 0;
float dTime;
uint32_t Print_time=0;
// Скорости.
float CommandSpeedL = 0, CommandSpeedR = 0, CommandSpeed=0;
// Измеренные скорости.
float RealSpeedL = 0, RealSpeedR = 0;
// Мощность на моторы.
float Lmotor = 0, Rmotor = 0;
// Массивы предыдущих значений энкодеров по колесам
// хранятся за последние 50 миллисекунд с интервалом 5 миллисекунд.
long OldLeftEncoder[10], OldRightEncoder[10];
//======================
void setup() {
//Serial для всязи с ПК
Serial.begin(115200);
// Коэффициенты ПИД-регулятора для режима удержания скорости
// вращения колеса
// smKp - к ошибке скорости колеса,
// smKd - к производной от ошибки скорости колеса
// (скорость изменения ошибки скорости колеса)
smKp = 0.005; smKd = 0.00025;
// Инициализируем энкодеры, инициализируем моторы.
encoder_setup();
// Принудительное обнуление элементов массива.
for (int i = 0; i < 10; i++)
{
OldLeftEncoder[i] = 0;
OldRightEncoder[i] = 0;
}
Delay_time = millis();
}
//=============================================
void loop()
{
// Текущее время.
millis_ = millis();

257

258

Глава 16

if (millis_ - Delay_time >= 5)
{
// Время между опросами в секундах.
dTime = float(millis_ - Delay_time) * 0.001;
// Снимаем показания с энкодеров.
LeftEncoder = readLeftEncoder();
RightEncoder = readRightEncoder();
static int i = 0; // Счетчик для массива предыдущих значений
// с энкодеров
// Нахождение текущей скорости левого колеса.
// Скорость измеряем - расстояние, пройденное колесом за 50 миллисекунд.
// При меньшем времени возрастают погрешности
// за счет дискретности энкодеров.
RealSpeedL = float(LeftEncoder - OldLeftEncoder[i]) / (10.0 * dTime);
// Нахождение текущей скорости правого колеса.
RealSpeedR = float(RightEncoder - OldRightEncoder[i]) / (10.0 * dTime);
// Реальная средняя скорость.
CommandSpeedL = CommandSpeed;
CommandSpeedR = CommandSpeed;
// Расчет ПИД-регулятора подаваемой мощности ШИМ на левый мотор.
Lmotor = PID_moveSpeed_calcLEFT(RealSpeedL, CommandSpeedL, Lmotor, dTime);
// Расчет ПИД-регулятора подаваемой мощности ШИМ на правый мотор.
Rmotor = PID_moveSpeed_calcRIGHT(RealSpeedR, CommandSpeedR, Rmotor, dTime);
// Подача мощности на моторы.
motors_power(int(Lmotor), int(Rmotor));
// В память текущие значения на энкодерах.
OldLeftEncoder[i] = LeftEncoder;
OldRightEncoder[i] = RightEncoder;
Delay_time = millis_;
i++; if (i > 9) i = 0;
}
// Если пришли данные по Bluetooth.
while (Serial.available())
{
// Записываем, что пришло по Serial, в переменную bt_input.
char bt_input = (char)Serial.read();
switch (bt_input) {
// Вперед
case 'F':
CommandSpeed=constrain(MAXMOVESPEED,CommandSpeed-100,CommandSpeed+100);
break;
// Назад.
case 'B':
CommandSpeed=constrain(-MAXMOVESPEED,CommandSpeed-100,CommandSpeed+100);
break;

Балансирующий робот

259

// Стоп.
case 'S':
CommandSpeed = 0;
break;
}
}
if (millis_ - Print_time >= 200)
{
Print_time=millis_;
Serial.print(CommandSpeed); Serial.print(" ");
Serial.print(CommandSpeedL); Serial.print(" ");
Serial.println(CommandSpeedR);
}
}

Когда моторы настроены, нужно поэтапно настроить коэффициенты для регулятора
робота:
1. Настройка дифференцирующего звена по углу:
Kp_ea=0, Kd_ea="Подбираем", Kp_s=0, Kd_s=0, Ki_s=0

Добиваемся того, чтобы робот в состоянии балансировки (поднятый) активно
сопротивлялся изменению угла наклона. Вы хотите уронить робота, а он катится
в сторону наклона, мешая вам его уронить.
2. Настройка звена по углу:
Kp_ea="Подбираем", Kd_ea="Подобрали", Kp_s=0, Kd_s=0, Ki_s=0

Добиваемся того, чтобы робот в состоянии балансировки (поднятый) активно
держал нулевой угол наклона. Если балансирующего робота отпустить, он должен покатиться в сторону, всегда в одну — в ту, куда у него есть смещение центра тяжести относительно нуля по гироприбору. Добиваемся стабильного поведения робота — он не должен падать на участке несколько метров.
3. Настройка интегрального звена по ошибке скорости (в нашей программе коэффициент пропорциональности имеет отрицательный знак):
Kp_ea="Подобрали",Kd_ea="Подобрали", Kp_s=0, Kd_s=0, Ki_s="Подбираем"

Добиваемся того, чтобы робот в состоянии балансировки (поднятый), но придерживаемый сверху рукой, совершал колебания, как маятник с амплитудой
~50 см. Дело в том, что без пропорционального звена по ошибке скорости интегральное звено переполняется, а это приводит к колебаниям. Для гашения колебаний вводим следующее звено.
4. Настройка пропорционального звена по ошибке скорости (в нашей программе
коэффициент пропорциональности имеет отрицательный знак):
Kp_ea="Подобрали",Kd_ea="Подобрали",Kp_s="Подбираем",Kd_s=0,Ki_s="Подобрали"

Добиваемся того, чтобы робот в состоянии балансировки (поднятый) стоял на
месте, совершая небольшие колебания. Робот должен стоять самостоятельно.

260

Глава 16

5. Настройка дифференцирующего звена по ошибке скорости (в нашей программе
коэффициент пропорциональности имеет отрицательный знак):
Kp_ea="Подобрали",Kd_ea="Подобрали",Kp_s="Подобрали",Kd_s="Подбираем",
Ki_s="Подобрали"

Звено убирает мелкие колебания от пропорционального звена по скорости, значение мало, результат иногда кажется субъективным (слабо улавливаемым).
Можно, увеличивая коэффициент, добиться «расколбаса» робота от этого звена,
а потом его уменьшать, пока робот не успокоится. Звено «работает» в большей
мере при движении робота.
В комплект примеров кода, зашитый в библиотеку mobrob3.zip, включена также
видоизмененная программа, в которую добавлена возможность управления от
модуля HC-06/05. В этой главе она не приведена, т. к. мало отличается от приведенной ранее, кроме наличия обработки поступивших команд-символов от Serialпорта.

Выводы
В этой главе мы создали балансирующего робота, рассмотрели теорию баласировки и применили ее на практике. Робот балансирует, а во вторую версию программы
балансировки (см. листинг 16.3 в комплекте примеров кода, зашитом в библиотеку
mobrob3.zip) добавлено также управление его перемещением.
Рекомендуем вам и дальше развивать полученные знания и умения, не бояться
экспериментировать и пробовать свои версии роботов и алгоритмов.

ПРИЛОЖЕНИЕ
ОПИСАНИЕ
ЭЛЕКТРОННОГО АРХИВА

Электронный архив с материалами к этой книге можно скачать с сервера издательства «БХВ» по ссылке https://zip.bhv.ru/9785977517034.zip или со страницы книги
на сайте https://bhv.ru/.
В архиве (табл. П.1) содержится библиотека mobrob3.zip, которая должна быть установлена стандартными средствами Arduino IDE: выполнить команду меню Скетч |
Подключить библиотеку | Добавить .ZIP библиотеку (рис. П.1) и выбрать с диска файл mobrob3.zip (рис. П2). После этого вам станут доступны библиотечные
модули, описанные в этой книге, а также из меню Arduino IDE Примеры — все
примеры из книги (рис. П.3) и все программные модули, которые приведены в книге

Рис. П.1. Установка в Arduino IDE библиотеки из ZIP-архива: этап 1

262

Рис. П.2. Установка в Arduino IDE библиотеки из ZIP-архива: этап 2

Рис. П.3. Пример открытия в Arduino IDE скетча из книги (листинг 11.3 из главы 11)
после установки библиотеки mobrob3.zip

Приложение

263

Описание электронного архива

и некоторые в ней не приведенные (вариации по устройствам удаленного управления).
Кроме библиотеки программ в архив включены файлы с 3D-моделями корпусов и
колес роботов, которые использовались при написании глав и создании скетчей.

Таблица П.1. Содержание электронного архива, сопровождающего книгу
Файл

Описание

Главы

mobrob3.zip

Библиотека и примеры из книги

5–16

robotstl.zip

Корпуса и колеса роботов в STL формате для 3D-печати

5–16

ПРЕДМЕТНЫЙ УКАЗАТЕЛЬ

A
Arduino UNO 21

B

I
IR-канал 138
IR-компоненты 114
IR-приемник 22, 114, 116
IR-пульт 114, 138

Bluetooth-контроллер 124, 125

А
Акселерометр 14, 139, 141, 143–147, 157
Активные флюсы 39
Алгоритм
◊ обхода лабиринта 206
◊ объезда препятствий 193
◊ тестовых движений робота 109
Алкалиновые батареи 42
Аналого-цифровые преобразователи 20
Аудиоканалы 22

Б
Беспроводные компьютерные сети 23
Библиотеки 70
Блок-схема алгоритма 54, 65
Бокорезы 32, 35

В
Видеокамера 19
Визуальные каналы 22
Внутреннее сопротивление 41
Встроенные функции 70
Выбор
◊ паяльника 37
◊ припоя 38
◊ ходовой части 74
Выключатель 28

Г
Гирокомпас 14
Гироскоп 139
Гироскоп-акселерометр MPU-6050 139, 157
Гусеничная ходовая часть 74, 75

266

Д
Дальномер 14, 174
◊ HC-SR04 177, 187
Датчик 13
◊ влажности 18, 19
◊ газа 18
◊ движения 18
◊ касания 14
◊ наличия освещенности 159
◊ препятствия 15, 16, 244
◊ расстояния 14
◊ температуры 14
◊ угла поворота 17
◊ шума 14
Двигатели постоянного тока с редуктором
76
Двоичное представление чисел 53
Десятичная система счисления 53
Детектор шума 17
Дисплей 24
Драйвер двигателей 33, 92, 102

Е
Емкость аккумуляторной батареи 41

Ж
Желеобразные флюсы 39

З
Заготовка программы 56
Задание номера порта 58
Закон Ома 40
Защита роботов от электромагнитных
помех 129

И
Измеритель освещенности 14
Инкрементальный энкодер 227
Информационно-измерительная система 13
◊ роботов 19
Инфракрасные каналы 22
Инфракрасный
◊ датчик
 отражения TCRT 5000 161
 препятствия 16
 расстояния 16

Предметный указатель
◊ канал 113
◊ светодиод 15
Исполнительная система 24

К
Канал Bluetooth 23, 138
Клеммные соединения 33
Клеммы
◊ Dupont 33
◊ с болтовым зажимом 33
Коды кнопок IR-пульта 118, 120
Колесная ходовая часть с дифференциалом
75
Колесные ходовые части с мотором 75
Компьютерная программа 53
Конденсаторный микрофон 17
Контроллер
◊ Arduino 20
◊ Bluetooth HC-05 126

Л
Летающие роботы 76
Линейный алгоритм 54
Линза Френеля 18
Литиевые аккумуляторы 93
Литий-ионные аккумуляторы 42, 43
Лужение провода 37

М
Магазин приложений Google Play Маркет 123
Магнитный датчик 14
Манипуляторы 24
Микроконтроллер 20
Микросхема
◊ L7805CV 44
◊ LM2596 45
◊ LM317T 46
◊ MPU-6050 139, 143–146, 148, 149, 157
◊ КР142ЕН5А 44
Многожильные провода 31
Модули Wi-Fi 23
Модуль HC-05 124, 125

Н
Недостатки клеммных соединений 33
Никель-металлогидридные аккумуляторы
42

267

Предметный указатель
Номинальное напряжение 40
Номинальный ток 41

О
Обмен информацией с ПК 61
Объектно-ориентированное
программирование 71
Объявление переменных 62
Одножильные провода 31
Оперативная память 20
Операторы организации циклов 68
Операционная система Android 23
Оптический рефлекторный датчик
расстояния 16

П
Пайка 35, 36
Паяльная кислота 39
Паяльник 35, 37, 38
Перезаряжаемые элементы питания 42
Переменные 62
◊ с плавающей точкой 63
Переменный резистор 17
ПИД-регулятор 150
Плата
◊ IR-приемника 115
◊ драйвера двигателей 92
Повышающие импульсные стабилизаторы
45
Подключение контроллера Arduino к
компьютеру 56
Пожарная сигнализация 18
Поиск выхода из лабиринта 201
Получение данных от MPU-6050 146
Пороговый
◊ датчик освещенности 15
◊ уровень срабатывания датчика 15
Постоянная память 20
Правила поведения робота 20
Припой 35
Проводники электрического тока 31
Проводные каналы 22
Программа
Fritzing 14
◊ балансировки с использованием
комплементарного фильтра 249, 256
◊ измерения расстояний 178
◊ инсталляции Arduino IDE 55
◊ переименования робота 127

Программатор 58
Процессор 20

Р
Радиоканал
◊ Bluetooth 113
◊ Wi-Fi 113
Радиоканалы 22
◊ связи 23
Радиоуправляемые квадрокоптеры 76
Разветвляющийся алгоритм 54
Расстояние срабатывания 15
Редукторные передачи 25
Резистор 27
Робот
◊ исполнительная система 13
◊ механика 13
◊ система принятия решений 13
◊ система связи 13
◊ система энергоснабжения 13

С
Сборка робота 90
Светодиод 15, 28, 60, 160, 161
Сенсор 13
Сервомотор 26
◊ SG90 188
Сервомоторы постоянного вращения 76
Система
◊ машинного зрения 14
◊ полива растений 18
◊ принятия решений 20
◊ связи 22
◊ энергоснабжения 27
Соединение
◊ клеммами 33
◊ скруткой 32, 33
Солевые батареи 42
Специализированные микросхемы
драйверов двигателей 78
Способы дистанционного управления
роботом 113
Среда Arduino IDE 55
Стабилизатор тока 46
Стабилизаторы питания 44
Стандарт
◊ AA 41
◊ AAA 41
Счетчик команд 53

268

Т
Текстовая форма алгоритма 55
Температурный датчик 14
Тестовая программа движений робота 110,
111
Типоразмер 18650 41
Типоразмеры элементов питания 41
Типы данных переменных 63
Триангуляционный принцип измерений 16
Тумблер 28

У
Укладка проводов 107
Ультразвуковой
◊ дальномер HC-SR04 178, 192
◊ датчик расстояния 16
Управление роботом
◊ по Bluetooth 114
◊ роботом по IR-каналу 113
◊ роботом по каналу Bluetooth 123
Управляющие команды 22
Условные операторы 64
Установка нестандартного драйвера 58
Устранение неполадок сборки 191

Ф
Флюс 39
Фотодиод 159
Фоторезистор 15, 160
Фототранзистор 15, 161
Функции 70
◊ движений 109

Х
Ходовая часть 24
◊ робота 74

Предметный указатель

Ц
Целочисленные переменные 63
Цифровые порты 21, 61

Ч
Четырехколесный робот 76

Ш
Шаговые двигатели 26, 76, 77
Широтно-импульсная модуляция 83, 187
Широтно-импульсные генераторы 20

Э
Электрическая мощность 40
Электрические двигатели 24
Электрический двигатель постоянного тока
25
Электромагнитные наводки 80, 92
Электронный
◊ акселерометр 139
◊ архив 10, 61, 86, 261
◊ гироскоп 139, 142, 143
◊ гироскоп-акселерометр 139
Элементы питания робота 93
Энкодер 17
Этапы пайки 36

Я
Язык С++ 55