/
Tags: самодействующие системы кибернетика
ISBN: 978-5-9775-6647-6
Text
Михаил Момот
Мобильные роботы
на базе ESP32
в среде Arduino IDE
Санкт-Петербург
«БХВ-Петербург»
2020
УДК 007.52
ББК 32.816
М76
М76
Момот М. В.
Мобильные роботы на базе ESP32 в среде Arduino IDE. — СПб.: БХВ-Петербург,
2020. — 272 с.: ил.
ISBN 978-5-9775-6647-6
Руководство для начинающих конструкторов написано в форме практических
проектов по построению мобильных роботов на новых высокоскоростных
контроллерах ESP32. Использована единая базовая двухколесная конструкция на
популярных высокоточных шаговых моторах. Все детали вырезаны из фанеры, их
также можно напечатать на 3D-принтере. Описаны наиболее распространенные
компоненты. Доступно изложено проектирование механики, приводов, элементов питания и стабилизации напряжения, электронных схем, программирование
в среде Arduino IDE на примерах конструирования роботов различной функциональности. Особое внимание уделено созданию двухколесного балансирующего
робота. Описано взаимодействие с датчиками нажатия (кнопка) и расстояния,
гироскопом и акселерометром.
Электронный архив, находящийся на сайте издательства, содержит чертежи
деталей для печати на 3D-принтере и листинги.
Для читателей, интересующихся электроникой, робототехникой
и программированием микроконтроллеров
УДК 007.52
ББК 32.816
Группа подготовки издания:
Руководитель проекта
Зав. редакцией
Компьютерная верстка
Дизайн серии
Оформление обложки
Евгений Рыбаков
Екатерина Сависте
Людмилы Гауль
Марины Дамбиевой
Карины Соловьевой
«БХВ-Петербург», 191036, Санкт-Петербург, Гончарная ул., 20.
ISBN 978-5-9775-6647-6
© ООО «БХВ», 2020
© Оформление. ООО «БХВ-Петербург», 2020
ОГЛАВЛЕНИЕ
Введение ................................................................................... 7
Глава 1. Контроллеры на базе ESP32
и их применение в робототехнике ....................................11
От Ардуино к ESP32 ............................................................................... 11
Сравнение характеристик контроллеров Arduino Nano,
Arduino Mega2560 и микроконтроллера ESP32 ....................... 12
Модули на основе ESP32 для создания прототипов .............. 17
ESP32 с 4 Мбайт Flash-памяти .........................................................................17
ESP32 Lolin Lite .......................................................................................................17
ESP32 LOLIN D32 ....................................................................................................18
ESP32 AI-Thinker с видеокамерой ..................................................................18
Функциональная схема ESP32 и его распиновка .................... 21
Среды и средства программирования ESP32 ........................... 21
Arduino IDE ...............................................................................................................21
Экосистема Espruino ...........................................................................................21
Экосистема Whitecat ...........................................................................................24
Операционная система FreeRTOS ..................................................................26
Язык программирования MicroPython........................................................26
Глава 2. Настройка среды программирования
Arduino IDE для работы с ESP32 ......................................... 29
Установка поддержки в Arduino IDE контроллеров ESP32.. 29
Установка среды Arduino IDE ...........................................................................29
Подключение репозитория .............................................................................30
Установка платы ESP32 из Менеджера плат..............................................32
Выбор платы и настройка параметров .......................................................33
Тестирование работы ESP32.............................................................. 35
Подключение к компьютеру и выбор COM-порта .................................35
Установка AdvancedWebServer .......................................................................35
Модификация проекта AdvancedWebServer: вывод графика
температуры от внутреннего датчика ESP32 ...........................................37
Тестирование поддержки Bluetooth.............................................................39
4
Оглавление
Глава 3. Выбор двигателей, тесты программного
управления шаговыми моторами .....................................41
Моторы коллекторные, бесколлекторные, шаговые
и сервомоторы ........................................................................................ 41
Коллекторные моторы........................................................................................41
Бесколлекторные моторы .................................................................................42
Шаговые моторы ...................................................................................................42
Сервомоторы ..........................................................................................................42
Обоснование выбора шагового мотора .....................................................42
Основы работы шаговых моторов ................................................. 43
Принцип работы ...................................................................................................43
Выбор конкретной модели ...............................................................................44
Драйвер DRV8825 и особенности его использования.........................45
Практическая проверка работы шагового мотора ................ 46
Сборка тестовой схемы .....................................................................................46
Регулировка тока драйвера .............................................................................49
Программа управления......................................................................................50
Разгон/торможение, перезапуск....................................................................55
Внешнее интерактивное управление скоростью, количеством
и направлением шагов, ускорением ............................................................63
Глава 4. Собираем базовую модель .................................. 69
Сборка механики робота .................................................................... 69
Конструкция и крепление шаговых моторов...........................................69
Требования к колесам .........................................................................................71
Сборка корпуса .....................................................................................................73
Сборка электроники ............................................................................. 76
Особенности установки контроллера ESP32 на макетную плату ...76
Сборка схемы электропитания .......................................................................78
Схема управления.................................................................................................82
Тестовая программа ............................................................................. 83
Работа с прерываниями ...................................................................... 84
Базовая программа управления роботом .................................. 89
Определение моторов в программе (файл motorstep.h)....................89
Настройка генерации шагов (файл irq_robot.h) ......................................93
Создание примитивов движений (файл move_case.h)...................... 105
Выбор системы управления и основная программа для робота 110
Оглавление
5
Пробный старт.......................................................................................114
Неполадки и пути их устранения..................................................115
Выводы ......................................................................................................116
Глава 5. Учим робота самостоятельно повторять
пройденный путь .................................................................117
Обоснование системы хранения записанного маршрута 119
Работа с энергонезависимой памятью контроллера ESP32 ........... 120
Обоснование выбора элементов управления .......................124
Выбор способа переключения режимов ................................................ 124
Выбор индикатора режима ........................................................................... 128
Установка элементов управления на робота ..........................131
Кнопки выбора режима................................................................................... 131
Информационный адресный светодиод ................................................. 134
Программирование ............................................................................135
Описание алгоритма работы робота ........................................................ 135
Особенности программы .............................................................................. 142
Глава 6. Автономное движение, обход препятствий
и прохождение лабиринта .................................................151
Схемотехника ........................................................................................156
Программная реализация ...............................................................158
Алгоритм измерения расстояния ............................................................... 158
Sonar() — функция одновременной работы
с четырьмя сонарами ...................................................................................... 160
Основная программа ...................................................................................... 164
Самостоятельное задание ...............................................................171
Выводы ......................................................................................................171
Глава 7. Робот телеприсутствия ........................................173
Смартфон на роботе в роли камеры ..........................................173
Настройка смартфона управления ............................................................ 176
Соединение смартфонов ................................................................................ 177
Соединение смартфона на роботе с роботом ...................................... 178
Управляем роботом через Интернет ........................................................ 182
Модуль ESP32-CAM на роботе в роли камеры........................184
6
Оглавление
Модернизация робота ..................................................................................... 190
Подключение модуля ESP32-CAM к роботу ........................................... 198
Глава 8. Балансирующие роботы..................................... 205
Гироскоп-акселерометр-термометр MPU-6050 .....................206
Шина I2C .................................................................................................................. 207
Электронный гироскоп ................................................................................... 208
Электронный акселерометр ......................................................................... 209
Шкала значений MPU-6050 ............................................................................ 211
Получение и обработка данных от MPU-6050 ...................................... 211
Основы регулирования: ПИД-регулятор ..................................220
Настройка ПИД-регулятора балансирующего робота........224
Как остановить робота? .................................................................................. 229
Подключаем внешнее управление и подъемный рычаг
(окончательная версия программы) ......................................................... 236
Приложение. Содержание файлового архива .............. 257
Предметный указатель ...................................................... 263
ВВЕДЕНИЕ
Здравствуйте, дорогие читатели!
Возможно, вы уже знакомы с моей предыдущей, выдержавшей уже два издания
книгой «Мобильные роботы на базе Arduino»1. В чем же отличие от нее этой
новой книги — «Мобильные роботы на базе ESP32»?
Это не просто замена плат Arduino Nano или Uno на ESP32 и повторение старых
проектов на новом микроконтроллере. В новой книге мы раскроем возможности современных микроконтроллеров типа ESP32, которые существенно богаче
как по производительности, так и по функционалу, что позволяет создавать качественно новых мобильных роботов с совершенно новым интересным функционалом.
Чтобы вы в своих проектах научились эти возможности реализовывать, мы решили продемонстрировать их на примере создания мобильных роботов, как
наиболее близких нам по духу.
Вы освоите работу в среде Arduino с контроллером ESP32, разберетесь с особенностями управления шаговыми моторами, научитесь создавать и использовать
функции, которые запускаются в фоновом режиме через определенные промежутки времени (прерывание по таймеру), создадите робота с камерой, управляемого из любой точки планеты через Интернет, запустите своего балансирующего двухколесного робота и поймете, как он работает.
Для таких роботов требуется скорость обработки информации, которую невозможно получить при использовании 8-разрядных Arduino-контроллеров
Arduino Uno или Mega и пр. Но контроллер ESP32 не только быстро работает,
он также обладает рядом других полезных качеств, отраженных в этой книге:
поддерживает программирование в среде Arduino IDE, имеет 32 разряда и частоту 240 МГц, содержит на борту современные интерфейсы Wi-Fi и Bluetooth.
1
См. http://www.bhv.ru/books/book.php?id=199367.
Введение
8
Кроме того, ESP32 обеспечен масштабной поддержкой производителя и сообщества Arduino (большинство библиотек для периферии Arduino адаптированы
для ESP32). Важно также учитывать низкую стоимость ESP32, сравнимую со
стоимостью Arduino Uno.
Книга будет полезна тем, кому интересно освоить новый микроконтроллер, но
нет желания покидать уютную среду Arduino, поможет она также и начинающим
робототехникам, поскольку создать управляемого робота на базе ESP32 проще,
чем на базе контроллера Arduino Nano или Uno, — им не потребуется искать и
подключать для организации управления внешние модули. Схемотехника рассматриваемых в книге роботов предельно проста — упор сделан на беспаечный
монтаж при помощи макетных плат и соединительных проводов.
Книга не содержит рассмотрения основ электрики, пайки и программирования,
и если вам это необходимо, обратитесь к уже упомянутой книге «Мобильные
роботы на базе Arduino» или к книге Марко Шварца «Интернет вещей с
ESP8266»2.
Книгу традиционно сопровождает файловый архив, содержащий полные листинги программ и чертежи элементов корпуса робота, описанные в книге
(см. приложение). Если вы начинающий робототехник, которому сложно самостоятельно составить программу по описанию из книги, можете взять готовую
работающую из архива. Скачать этот архив можно с FTP-сервера издательства
«БХВ-Петербург» по ссылке ftp://ftp.bhv.ru/9785977566476.zip или со страницы книги на сайте www.bhv.ru.
Книга разбита на главы:
• первая глава — обзорная и знакомит с особенностями микроконтроллера
ESP32;
• вторая глава посвящена настройке оболочки Arduino IDE для поддержки
ESP32;
• третья глава — это выбор моторов для мобильного робота и изложение
основ управления шаговыми моторами;
• в четвертой главе мы соберем базовую платформу робота и научимся
управлять роботом удаленно;
• пятая глава покажет, как научить робота запоминать и повторять пройденный путь;
• шестая глава описывает применение алгоритма одновременной обработки
данных с нескольких ультразвуковых датчиков расстояния типа HC-SR04.
В результате мы получим робота, способного обходить препятствия;
2
См. http://www.bhv.ru/books/book.php?id=203565.
Введение
9
• седьмая глава вводит нас в мир роботов телеприсутствия. Вы создадите робота с видеокамерой, управлять которым можно из любой точки мира;
• заключительную восьмую главу мы традиционно посвятили балансирующим роботам, и благодаря использованию шаговых моторов и высокой
скорости ESP32 робот у нас получился очень интересный.
Мы рассчитываем, что время и силы, потраченные вами на создание роботов по
приведенным в книге проектам, будут не напрасны и позволят вам в будущем
создавать собственные интересные проекты на базе идей, составляющих и алгоритмов, приведенных в книге.
В любом случае, если у вас возникнут вопросы, вы можете смело обращаться
к нам по адресу momotmvu@yandex.ru, мы всегда рады ответить на ваши вопросы, постарайтесь только четко их сформулировать и привести достаточный
объем информации, чтобы мы смогли понять их суть. В этом случае вам гарантирован ответ.
С уважением, Михаил Момот
ГЛАВА 1
КОНТРОЛЛЕРЫ НА БАЗЕ ESP32 И ИХ
ПРИМЕНЕНИЕ В РОБОТОТЕХНИКЕ
От Ардуино к ESP32
Сложные «думающие» роботы управляются сложными и дорогими компьютерами. Простые мобильные роботы, о которых пойдет речь в книге, управляются более простыми и недорогими микроконтроллерами и контроллерами.
Микроконтроллеры также могут успешно управлять несложными промышленными роботами.
Контроллер по структуре очень похож на компьютер (рис. 1.1), но, как правило, имеет больше ограничений, и специфика его применения не столь универсальна. Отличается контроллер от микроконтроллера тем, что все составляющие микроконтроллера расположены на одной микросхеме. Например,
ATmega328 — микроконтроллер, потому что его микропроцессор, оперативная
память и постоянная память (аналог жесткого диска компьютера) скомпонованы в пределах одной микросхемы.
Контроллер
прерываний
Контроллер
последовательного
интерфейса UART
Оперативная
память
Таймеры
Процессор
Порты
ввода/вывода
Энергонезависимая
память
Рис. 1.1. Упрощенная структура контроллера
12
Глава 1
На микроконтроллерах ATmega основана популярная аппаратная вычислительная платформа контроллеров Arduino. Контроллеры Arduino представляют собой
платы с распаянным микроконтроллером ATmega и всей необходимой для него
обвязкой, регулятором напряжения и USB-UART-мостом. Все контакты платформы выведены на края платы и, как правило, уже оборудованы разъемами.
ESP32 — современный микроконтроллер, обладающий расширенным функционалом и многообещающими возможностями. В состав ESP32 входят модули
Wi-Fi и Bluetooth, два процессорных ядра и богатый набор периферии. Именно
поэтому применение его в робототехнике вполне обоснованно.
Замечу, что мы не призываем отказаться от использования Arduino Nano и Uno,
но требования, предъявляемые к проектам, рассмотренным в этой книге, упираются в нехватку ресурсов ATmega328 и даже ATmega2560 (Arduino Mega) по
скорости, по разрядности и по количеству встроенных интерфейсов. Arduino
Nano и Uno применяют для решении задач, которые им по плечу, а при помощи
ESP32 можно существенно расширить арсенал простых в монтаже, использовании и программировании контроллеров, «заточенных» на решение современных относительно скоростных задач. Вот несколько параметров, по которым
ESP32 превосходит распространенные недорогие контроллеры Arduino: скорость опроса датчиков, скорость и качество управления шаговыми двигателями,
возможность создания веб-сервера с поддержкой сетевого интерфейса Wi-Fi.
Впрочем, рассматривать возможности лучше всего в сравнении с аналогами.
Сравнение характеристик контроллеров
Arduino Nano, Arduino Mega2560
и микроконтроллера ESP32
Первая версия микроконтроллера ESP32 была выпущена компанией
Espressif Systems (Китай, головной офис находится в Шанхае) в 2015 году.
Микроконтроллер несколько раз незначительно модернизировался и к настоящему моменту представляет собой двухъядерный модуль, способный работать
на частоте 240 МГц, имеющий 512 Кбайт встроенной оперативной памяти (той
памяти, в которой располагаются код программы и данные при выполнении),
а также большое количество поддерживаемых на аппаратном уровне интерфейсов. В то же время ESP32 рассчитан на работу с внешней энергонезависимой
памятью.
ESP32 — микроконтроллер 32-разрядный, он может обработать за один такт
(1/240000000 доля секунды) число длиной 4 байта (32 двоичных разряда), тогда
как микроконтроллеры ATmega328, стоящие на платах Arduino Nano и Uno,
Контроллеры на базе ESP32 и их применение в робототехнике
13
8-разрядные и обрабатывают за один такт 1 байт (8 двоичных разрядов). Это
означает, что скорость обработки информации контроллером ESP32 превышает
таковую в ATmega328 (Arduino Nano и Uno) приблизительно в 60–100 раз. В отличие от большинства недорогих контроллеров, предназначенных для проектов
Arduino, микроконтроллер ESP32 обеспечивает поддержку питания 3,3 вольта.
Важной особенностью контроллеров, основанных на ESP32, являются встроенные беспроводные интерфейсы, благодаря чему весьма легко создать на основе
такого контроллера простой веб-сервер, обращаться к нему через стандартный
браузер смартфона или компьютера по IP-адресу во внутренней сети вашего
дома (при подключении к Wi-Fi) и получать полезную информацию от подключенных к нему датчиков. Можно также организовать прием и передачу информации по каналу Bluetooth.
Ре ма рка от ав тора
Мое знакомство с контроллерами фирмы Espressif, которые поддерживают создание проектов Arduino, началось с другого контроллера — ESP8266, но работа с ним
в среде Arduino IDE при создании мобильного робота не заладилась, поскольку для
него оказалось недостаточно свободных выводов GPIO1, — вернее, они были, но параллельно использовались для внутреннего потребления. Кроме того, программный
код стандартных библиотек содержал ошибки и пробелы. Я пытался создать роботафутболиста, он должен был гонять мигающий в инфракрасном свете мяч. После пары
дней экспериментов мне пришлось вернуться к использованию Arduino Nano, и я достаточно быстро реализовал задуманное, используя прямой доступ к регистрам ввода/
вывода и повысив скорость аналого-цифрового преобразования информации от ИК
фототранзисторов, выступающих в роли датчиков мяча, уменьшив его точность. Да,
оптимизация программного кода нередко способна увеличить производительность
программы чуть ли не в 10 раз.
Когда же была представлена поддержка платформы Arduino для чипа ESP32, я как раз
занимался разработкой балансирующего робота на шаговых моторах. Для раздельного управления работой пары «шаговиков» потребовался контроллер с наличием
двух 16-разрядных таймеров — по таймеру для управления каждым шаговым мотором. Arduino Nano и Uno подобными ресурсами не располагали, следовало сделать
выбор между Arduino Mega2560 и ESP32: первый имел два 16-разрядных таймера,
а второй — четыре 64-разрядных. Покопавшись в примерах кода, я счел работу с таймерами в ESP32 более простой и удобной, все остальные параметры мне также понравились, а наличия огромного количества выводов GPIO, как в Arduino Mega, в проекте
балансирующего робота не требовалось. Робот был довольно быстро сконструирован
и отлично показал себя на испытаниях, что и послужило предпосылкой к созданию
других интересных проектов.
Представим сравнение ESP32 с двумя наиболее распространенными контроллерами, которые используются в проектах Arduino, в виде таблицы (табл. 1.1).
1
GPIO — интерфейс ввода/вывода общего назначения (от англ. General-Purpose Input/Output).
В общих случаях это информационные контакты, способные к приему или передаче данных.
В проектах Arduino обычно используется наименование PIN (пин).
Глава 1
14
Таблица 1.1. Сравнение характеристик контроллеров
Arduino Nano, Arduino Mega2560, ESP32
Arduino Nano
(ATmega328)
Arduino Mega
(Mega2560)
ESP32
Разрядность данных
8
8
32
Тактовая частота, МГц
16
16
240
Размер ОЗУ, Кбайт
2
8
512
1/32
4/256
4000–16 000
(внешняя память)
Количество контактов GPIO
общее
14
54
до 25 (зависит от
реализации)
16-разрядные таймеры общего
назначения
1
4
—
8-разрядные таймеры общего
назначения
2
2
—
64-разрядные таймеры общего
назначения
—
—
4
1 (D0, D1)
4 (D0–D1,
D19–D18,
D17–D16,
D15–D14)
3 (можно
выбирать GPIO)
10
10
12
~100
~100
~10
8
16
16–18
нет
нет
2 канала
Количество каналов ШИМ
(PWM)
6
14
16
Разрядность ШИМ (PWM), бит
8
8
1–15
~500
~500
1–78 125
(при разрядности
ШИМ 10)
1 (A4,A5)
1 (D20,D21)
2 (можно
выбирать GPIO)
Поддержка Bluetooth
—
—
1
Поддержка Wi-Fi
—
—
1
Рабочее напряжение, вольт
5
5
3,3
7–12
7–12
5
Размер энергонезависимой памяти: данные/код, Кбайт
Порт UART (аппаратный)
Разрядность АЦП, бит
Время одного аналого-цифрового
преобразования, мкс
Количество контактов GPIO
для АЦП
ЦАП (аналоговый сигнал на выходе)
Частота ШИМ (PWM), Гц
Кол-во интерфейсов I2C
Напряжение питания, подаваемое
на плату контроллера, вольт
Контроллеры на базе ESP32 и их применение в робототехнике
15
Рассмотрим теперь некоторые данные табл. 1.1 подробнее:
• оперативная память (эта быстрая память с прямым доступом служит для
хранения данных во время исполнения программы) — регламентирует размер исполняемой программы и размер используемых данных переменных
и массивов. Из табл. 1.1 можно сделать вывод, что размер ОЗУ ESP32 в
512 Кбайт позволяет разместиться весьма объемной программе (даже есть
примеры создания операционной системы Lua-RTOS для ESP32). Для наших будущих проектов такой объем оперативной памяти можно считать
достаточным и даже избыточным;
• энергонезависимая память (аналог жесткого диска компьютера) — этот тип
памяти в контроллерах ATmega разделен на сектор данных и сектор исполняемого кода. В ESP32 такого разделения нет, как нет и вообще внутренней
энергонезависимой памяти, — чип Flash-памяти здесь используется внешний, это отдельная микросхема на плате, не имеющая аппаратного разграничения на сектор данных и кода. По опыту, при использовании версии
модуля ESP32 c 4 Мбайт Flash-памяти для данных выделяется примерно
1 Мбайт — здесь можно сохранять данные, которые будут доступны после
перезагрузки и/или выключения питания. Например, информацию о пройденном роботом пути. Микроконтроллер ESP32 имеет 48 выводов, но не
все из них можно использовать в программе, — например, контакты GPIO
6–11 могут быть выведены на некоторых платах, но заняты для доступа к
данным распаянной на плате Flash-памяти;
• количество контактов GPIO — сравнимо с таковым в Arduino Nano, поэтому
практически все проекты, реализованные на Nano, могут быть легко перенесены на ESP32. При этом следует иметь в виду, что не все из 48 выводов
микроконтроллера ESP32 удается использовать в программе, — например,
контакты GPIO 6–11 могут быть выведены на некоторых платах, но заняты
для доступа к данным распаянной на плате Flash-памяти;
• таймеры — здесь все прекрасно. Отметим только, что 64-разрядные таймеры общего назначения ESP32 тактируются не частотой контроллера, а
частотой 80 МГц. Их использование очень удобно и не мешает другим ресурсам, — например, чтобы генерировать сигналы ШИМ, на Arduino Nano
задействуются системные таймеры. В ESP32 есть отдельный модуль генерации ШИМ (PWM). Таймеры можно применять для запуска небольших
участков программного кода (функций) через равные промежутки времени
в фоновом режиме. Основная программа на это время приостанавливается
(если используется одно и то же ядро), а после выполнения кода, привязанного к таймеру, продолжается. Эти программы называются обработчиками
прерываний, они должны быть максимально быстрыми, чтобы их постоянные вызовы не замедляли основную программу;
• аппаратный UART — это последовательный интерфейс (в ПК эту функцию
реализуют COM-порты) для связи с внешними устройствами. Он позволяет
16
•
•
•
•
•
Глава 1
осуществлять обмен информацией между различными контроллерами на
заданной скорости и задействуется при разработке систем, нуждающихся в
обмене информацией. Преимущество аппаратной реализации UART заключается в освобождении ресурсов ядра контроллера, поскольку используется
отдельный аппаратный модуль;
аналого-цифровой преобразователь, АЦП — позволяет измерять величину
напряжения на некоторых выводах GPIO и представлять его в числовом
виде. Для Nano и Mega — это числа от 0 до 1000 (измеряемое напряжение
0–5 В), для ESP32 — числа от 0 до 4000 (измеряемое напряжение 0–3,3 В).
По умолчанию в качестве опорного напряжения берется напряжение питания микроконтроллера. Чем выше разрядность, тем выше точность
преобразования. В некоторых проектах также важна скорость преобразования;
широтно-импульсная модуляция, ШИМ (от англ. Pulse-Width Modulation,
PWM) — используется в управлении мощностью моторов, сервомашинок,
бесколлекторных двигателей (например, в квадрокоптерах). Изменять
мощность, подаваемую на моторы, она позволяет за счет изменения длины
импульсов, генерируемых на заданной частоте. А то, что частота ШИМ может быть изменена в широких пределах, позволяет очень гибко подходить
к настройкам оборудования;
I2C-интерфейс (последовательная общая шина) — при использовании
ESP32 с Arduino IDE по умолчанию для шины I2C (библиотека Wire) задействуются стандартные выходы ESP32: GPIO 21 (SDA) и GPIO 22 (SCL).
При этом номера GPIO можно менять программно. I2C-интерфейс применяется различными «умными» датчиками и дисплеями, наличие двух подобных интерфейсов не лишнее — на один интерфейс можно подключить
одновременно до четырех устройств, если в проекте их больше. Если же
есть пара устройств с одинаковыми I2C-адресами, то использование ESP32
также оправданно;
цифроаналоговое преобразование, ЦАП — напряжение на входе измеряется
и представляется в числовой форме. В ESP32 есть два 8-разрядных канала
преобразования цифровых сигналов в аналоговые: DAC1 (GPIO25) и DAC2
(GPIO26). ЦАП можно применять при генерации звука или сигналов управления другими аналоговыми устройствами;
поддержка Bluetooth — весьма удобна в мобильных роботах и других домашних несложных автоматических устройствах. Часто применяется для
управления роботами со смартфона. Через Bluetooth можно получать от
робота статистическую информацию и другие данные. Для решения аналогичных задач с Arduino Nano/Mega потребуется дополнительно использовать внешний приемопередатчик, задействовать аппаратный UART-порт
или создавать программный UART на свободных выводах GPIO, что, конечно, менее удобно, чем наличие встроенного модуля;
Контроллеры на базе ESP32 и их применение в робототехнике
17
• поддержка Wi-Fi — встроенный модуль Wi-Fi позволяет легко подключать
устройства на ESP32 к Интернету. В частности, можно получить очень удобный интерфейс для удаленного контроля за мобильными роботами;
• напряжение питания — современная архитектура в совокупности с высокой скоростью работы на частоте 240 МГц привели к тому, что напряжение питания ESP32 было уменьшено до 3,3 В. Для совместимости устройств
с различными величинами логических уровней используются конвертеры
уровней или более простые компоненты вроде резисторного делителя или
опторазвязки. Отметим, что потребление тока ESP32 при работе с беспроводными интерфейсами возрастает, что накладывает дополнительные требования на стабильность электропитания. В ESP32 присутствует поддержка
ядра низкого энергопотребления. В частности, выводы GPIO, перенаправленные в подсистему с низким энергопотреблением RTC, могут использоваться, когда ESP32 находится в глубоком сне.
Мы рассмотрели основные возможности и особенности ESP32, на которые будем опираться в книге, но следует иметь также в виду, что реальные возможности
этого микроконтроллера еще шире. ESP32 по функционалу ближе к одноплатным компьютерам (Raspberry Pi), чем к контроллерам, но его низкая стоимость
и возможность программирования в простой среде Arduino IDE позволяют широко применять ESP32 в Arduino-проектах.
Познакомимся теперь с основными доступными на рынке модулями с микроконтроллером ESP32.
Модули на основе ESP32
для создания прототипов
ESP32 с 4 Мбайт Flash-памяти
Это наиболее распространенный контроллер (рис. 1.2). Однако с подобным модулем не всегда удобно работать — у него нет встроенных стабилизатора питания и конвертера USB-UART, благодаря которому контроллеры Arduino удобно
программируются через USB-интерфейс компьютера. Поэтому далее мы рассмотрим модули с уже распаянными на плате стабилизатором питания и USBUART.
ESP32 Lolin Lite
Типичный пример модуля ESP32, пригодного для установки на плате для макетирования (рис. 1.3). Защитный стальной экран на контроллере отсутствует,
Глава 1
18
контроллер распаян непосредственно на макетной плате, GPIO 21 не разведен.
Предполагалось, что подобные модули будут дешевле, но на деле разницы в цене
с другими модулями нет. Расстояние между рядами выводов Lolin Lite 23,5 мм,
что меньше, чем у других модулей, где это расстояние равно 26,5 мм, поэтому
его можно установить в стандартную плату для прототипирования (рис. 1.4).
ESP32 LOLIN D32
Этот модуль (рис. 1.5) отличается от предыдущего более прочным исполнением:
текстолит толщиной 1,7 мм против 1,1 мм у Lolin Lite, микроконтроллер защищен
металлическим кожухом, но его уже не установить в стандартную «макетку».
ESP32 AI-Thinker с видеокамерой
Интересные проекты можно придумать, используя модуль ESP32 с камерой
(рис. 1.6). Скорости ESP32 не всегда хватает для передачи картинки по Wi-Fi,
в то же время возможностей упрощенного внутреннего анализа изображения
должно быть вполне достаточно, чтобы, например, двигаться по нарисованным
на полу меткам.
Рис. 1.2. Модуль ESP32 со встроенной антенной и экранирующей крышкой
Контроллеры на базе ESP32 и их применение в робототехнике
19
Рис. 1.3. Модуль ESP32 Wemos Lolin Lite
Рис. 1.4. Модуль Wemos Lolin Lite установлен в плату для прототипирования (с обеих сторон
есть по одному контактному отверстию для подключения проводов)
Глава 1
20
Рис. 1.5. Модуль ESP32 LOLIN D32
Рис. 1.6. Модуль ESP32 AI-Thinker с видеокамерой
Контроллеры на базе ESP32 и их применение в робототехнике
21
Функциональная схема ESP32
и его распиновка
Рассмотрим функциональную схему микроконтроллера ESP32 (рис. 1.7). В левом верхнем углу схемы притаился блок Radio — ESP32 можно использовать
в качестве радиоприемника. Блок управления питанием содержит энергоэффективный сопроцессор, управляющий «спящим режимом». Блок периферии
включает IR-интерфейс, температурный сенсор, сенсоры касания. Конвертер
digital-to-analog применяется для воспроизведения звука. Остальные блоки мы
уже обсудили ранее.
На рис. 1.8 приведена типовая распиновка модуля ESP32 на плате. Контакты
GPIO, на которых возможна генерация ШИМ (PWM), обозначены знаком волны ~ , ADC — это контакты GPIO с возможностью аналогового ввода, VSPI и
HSPI — интерфейсы для подключения устройств, поддерживающих эти протоколы обмена, SDL и SDA — I2C-интерфейс, TX0 и RX0 — интерфейс UART0.
Еще раз отметим, что некоторые контакты GPIO для стандартных интерфейсов, закрепленные по умолчанию за одними номерами GPIO, можно изменять
в программе.
Среды и средства программирования ESP32
Arduino IDE
Программный код управления проектами, представленными в этой книге,
основан на языке программирования Wiring (упрощенная и оптимизированная
версия C++ для работы с портами ввода/вывода), более того, само программирование осуществляется в среде Arduino IDE, знакомой большинству мейкеров
(назовем так творческих людей, умеющих воплощать свои идеи) и описанной
во многих изданиях.
Однако серия контроллеров ESP32 достаточно универсальна и допускает использование иных сред и средств программирования. Кратко рассмотрим
основные из них.
Экосистема Espruino
Espruino — экосистема программирования микроконтроллеров на языке
JavaScript. Прошивка Espruino может исполнять JavaScript-код, подготовленный
в качестве прикладной программы.
22
Рис. 1.7. Функциональная схема микроконтроллера ESP32
Глава 1
Контроллеры на базе ESP32 и их применение в робототехнике
23
Рис. 1.8. Распиновка ESP32: ADC — аналоговые вводы, GPIO — стандартные контакты ввода/вывода
Глава 1
24
Рис. 1.9. Среда программирования Espruino
Эта экосистема состоит из следующих частей:
•
•
•
•
Espruino Web IDE — среда программирования (рис. 1.9);
Espruino Firmware — JavaScript-машина;
платы, совместимые с Espruino;
стандартная библиотека и внешние библиотеки.
Основатель и идеолог проекта Гордон Вильямс выпустил первую Espruino в
2013 году. Функции работы с GPIO в среде похожи на работу с Arduino, благодаря чему разобраться с новой платформой тем, кто знаком с Arduino, не составит
труда.
Экосистема Whitecat
IDE Whitecat поддерживает две программные модели: блоки и язык программирования Lua.
Визуальная среда программирования Whitecat (рис. 1.10) схожа со средами Scratch
и MIT App Inventor. В настоящее время подобные визуальные среды все чаще изучают в школах, поскольку процесс программирования становится наглядным.
Контроллеры на базе ESP32 и их применение в робототехнике
25
Рис. 1.10. Среда программирования Whitecat
Глава 1
26
Операционная система FreeRTOS
FreeRTOS — многозадачная операционная система реального времени (ОСРВ)
для встраиваемых систем. Ядро RTOS содержится только в трех файлах C-кода.
Под управлением этой операционной системы ESP32 становится многозадачным микрокомпьютером, имеющим свою оригинальную среду поддержки
пользовательских программ. Операционная система не имеет графической оболочки и управляется из командной строки (рис. 1.11).
Рис. 1.11. Командная строка FreeRTOS
Язык программирования MicroPython
ESP32 поддерживает современный и популярный язык программирования
MicroPython (облегченная версия языка Python). Прошивку можно загрузить
по ссылке: https://micropython.org/download#esp32.
Контроллеры на базе ESP32 и их применение в робототехнике
*
*
27
*
Среды Espruino, Whitecat, FreeRTOS и MicroPython позволяют создавать полноценные программы управления роботами, и каждая из них имеет свои достоинства и недостатки. В проектах этой книги мы будем использовать среду Arduino
IDE, как наиболее популярную. При наличии желания, описанные здесь проекты можно портировать (перевести) в указанные среды самостоятельно.
В следующей главе мы перейдем к практическому знакомству с ESP32 и первым
делом добавим поддержку микроконтроллеров ESP32 в Arduino IDE.
ГЛАВА 2
НАСТРОЙКА СРЕДЫ
ПРОГРАММИРОВАНИЯ ARDUINO IDE
ДЛЯ РАБОТЫ С ESP32
Arduino IDE — открытая среда программирования, поэтому по мере появления
нового «железа» быстро появляется и новый софт для него от независимых
разработчиков. Вот и для ESP32 инженеры-программисты из Espressif Systems
реализовали поддержку самостоятельно и разработали инструкции по добавлению ее в среду Arduino. Все достаточно просто, но никогда не будет лишним
обратить ваше внимание на все этапы настройки среды Arduino IDE для работы
с ESP32.
Установка поддержки в Arduino IDE
контроллеров ESP32
Установка среды Arduino IDE
Для подготовки программного обеспечения идем на сайт https://www.arduino.
cc/en/Main/Software (рис. 2.1), выбираем последнюю версию Arduino IDE, соответствующую вашей операционной системе (Windows, Linux или macOS), скачиваем ее и устанавливаем стандартными средствами ОС.
Для операционной системы Ubuntu среду Arduino IDE можно установить, выбрав ее пакет из репозитория в самой ОС, так что отсюда для этого ничего скачивать не нужно.
Дальнейшие действия производятся внутри оболочки Arduino IDE и не зависят
от типа ОС.
Глава 2
30
Рис. 2.1. Страница загрузки Arduino IDE
Подключение репозитория
Запускаем среду Arduino IDE (рис. 2.2) и выполняем команду меню Файл |
Настройки.
Рис. 2.2. Выбор меню Настройки
Настройка среды программирования Arduino IDE для работы с ESP32
31
В открывшемся окне настроек (рис. 2.3) нас интересует поле Дополнительные
ссылки для Менеджера плат. Нажатием на кнопку
, расположенную справа от этого поля, попадаем в окно ввода ссылок (рис. 2.4) и добавляем ссылку на ESP32: https://dl.espressif.com/dl/package_esp32_index.json — эта
ссылка подключает дополнительный репозиторий, созданный программистами Espressif и содержащий основные компоненты для работы с ESP32 в среде
Arduino IDE.
Кнопка добавления
ссылок
Рис. 2.3. Окно настроек Arduino IDE: добавляем ссылку
Рис. 2.4. Окно ввода дополнительных ссылок для Менеджера плат
Глава 2
32
Рис. 2.5. Окно настроек Arduino IDE: ссылка добавлена
Закрываем окно ввода ссылок кнопкой ОК, проверяем наличие введенной строки
в поле Дополнительные ссылки для Менеджера плат (рис. 2.5) и подтверждаем внесенные изменения, закрыв окно настроек нажатием на кнопку ОК.
Установка платы ESP32 из Менеджера плат
Для установки библиотек и компилятора для ESP32 выполним команду меню
Инструменты | Плата:. В открывшемся меню выбора плат выберем Менеджер
плат… (рис. 2.6). В строке поиска Менеджера плат введем: ESP32. Если предварительные настройки были сделаны правильно, получим в окне ссылку на
скачивание модулей поддержки ESP32 (рис. 2.7). Нажмем кнопку Установка
и дождемся завершения процесса скачивания недостающих файлов с сайта компании Espressif (рис. 2.8).
Рис. 2.6. Открываем Менеджер плат
Настройка среды программирования Arduino IDE для работы с ESP32
33
Кнопка установки
найденного пакета
Рис. 2.7. Найден пакет esp32 — можно приступать к его установке
Ждем завершения
процесса установки
Рис. 2.8. Этап установки: скачивание недостающих файлов
Выбор платы и настройка параметров
Плату из серии ESP32 следует выбрать ту, которая наиболее соответствует плате
вашего контроллера. К универсальным платам можно отнести WEMOS LOLIN32
(рис. 2.9) — большинство недорогих плат, для которых отсутствует наименование в списке плат, будут при этом нормально прошиваться и работать.
Глава 2
34
Устанавливаем для этой платы скорость загрузки на максимум (рис. 2.10).
Устанавливаем частоту Flash также на максимум (рис. 2.11).
Выбираем таблицу размещения данных во Flash по умолчанию (рис. 2.12).
Рис. 2.9. Выбор платы ESP32 WEMOS LOLIN32
Рис. 2.10. Установка скорости загрузки прошивки
Рис. 2.11. Выбор частоты Flash
Рис. 2.12. Выбор таблицы размещения информации
Настройка среды программирования Arduino IDE для работы с ESP32
35
Тестирование работы ESP32
Давайте сразу «возьмем быка за рога» и создадим веб-сервер с доступом по WiFi — конечно, для этого нужно, чтобы у вас была точка доступа Wi-Fi. Этот пример не потребует дополнительных модификаций модуля ESP32 и достаточно
объективно продемонстрирует его возможности.
В е б - се р в е р
Веб-сервер — это программа, которая организует подключение через веб-интерфейс
при помощи стандартного браузера компьютера или смартфона.
Подключение к компьютеру и выбор COM-порта
Подключаем плату ESP32 к компьютеру — в среде Arduino IDE появляется COMпорт — на рис. 2.13 это COM9 — его и выбираем.
Рис. 2.13. Плата подключена: выбор порта
Установка AdvancedWebServer
В главном меню программы (рис. 2.14) выполним команду Файл | Примеры |
WebServer | AdvancedWebServer и сохраним пример под любым именем.
В коде примера найдем строки:
и заменим фрагмент YourSSIDHere на имя вашей домашней сети Wi-Fi, а фрагмент YourPSKHere — на пароль к этой сети.
Если ваша сеть поддерживает автоматическую раздачу IP-адресов, то на этом
все... Нажимаем в окне Arduino IDE кнопку загрузки
. Если на вашей плате
ESP32 есть кнопка Boot, после появления информации о начале загрузки нажимаем эту кнопку на пару секунд.
Перейдем теперь в Монитор порта, выставим скорость 115 200 и нажмем на
ESP32 кнопку RST (Reset). Модуль перезагрузится — при этом, возможно, появятся нечитаемые символы, поскольку модуль еще не установил нужную скорость порта. Если подключение к сети Wi-Fi выполнено успешно, мы получим
IP-адрес нашего модуля (рис. 2.15).
Глава 2
36
Рис. 2.14. Выбор примера AdvancedWebServer
Рис. 2.15. Сообщения модуля ESP32 после загрузки примера AdvancedWebServer
Настройка среды программирования Arduino IDE для работы с ESP32
37
Перейдем в браузер и занесем полученный IP-адрес в адресную строку — произойдет подключение к веб-серверу, которым является сейчас наш ESP32. На
открывшейся в браузере странице вы увидите приветствие, время работы модуля и по случайному закону изменяющийся график (рис. 2.16). График не несет
какой-либо информации и лишь демонстрирует возможности.
Рис. 2.16. Страничка веб-сервера AdvancedWebServer
Задание
Найдите в тексте программы фрагмент Hello from ESP32!, замените его на
другой, перезалейте прошивку и оцените результат.
Модификация проекта AdvancedWebServer: вывод
графика температуры от внутреннего датчика ESP32
Сделаем наш сервер практически полезным, создав на его основе простейшую
метеостанцию. Для этого мы заменим значения, которые используются при построении графика случайных величин (см. рис. 2.16), на значение температуры
внутреннего датчика ESP32.
Добавим в начале программы описание функции получения температуры от
внутреннего датчика:
38
Глава 2
Изменим строки, отвечающие за построение графика, создадим массив int
y[39] из 39 элементов и, периодически заполняя его данными температуры,
построим по этому массиву график. После заполнения массив очищается, и построение графика начинается снова (рис. 2.17). Мы также конвертируем здесь
температуру в градусах Фаренгейта в величину градусов Цельсия (листинг 2.1).
Листинг 2.1. Веб-сервер, демонстрирующий температуру внутри
контроллера ESP32 (только измененная функция drawGraph())
void drawGraph() {
static int y[39]; static int i=0; String out = "";
char temp[100];
out += "<svg xmlns=\"http://www.w3.org/2000/svg\" version=\"1.1\"
width=\"400\" height=\"150\">\n";
out += "<rect width=\"400\" height=\"150\" fill=\"rgb(250, 230, 210)\"
stroke-width=\"1\" stroke=\"rgb(0, 0, 0)\" />\n";
out += "<g stroke=\"black\">\n";
y[i] = int((temprature_sens_read()-32)/1.8);
for (int x = 0; x < 38; x ++) {
sprintf(temp, "<line x1=\"%d\" y1=\"%d\" x2=\"%d\" y2=\"%d\"
stroke-width=\"1\" />\n", x*10+10, 140 - y[x], x*10 + 20, 140 - y[x+1]);
out += temp;
}
out += "</g>\n</svg>\n";
server.send(200, "image/svg+xml", out);
i++;
if(i==39) { i=0; memset(y, 0, sizeof(y)); }
}
Рис. 2.17. Веб-сервер: значения температуры заносятся в массив постепенно (нижняя линия
справа — это еще не заполненные температурой ячейки)
Настройка среды программирования Arduino IDE для работы с ESP32
39
Ф а й ло в ы й а рх ив
Напомню, что полные версии всех приведенных в книге программ (скетчей) содержатся в сопровождающем книгу файловом архиве (см. приложение).
Тестирование поддержки Bluetooth
В следующем примере мы запустим модуль Bluetooth (рис. 2.18), и нам понадобится смартфон.
Загрузим программу в ESP32, включим Bluetooth на смартфоне, найдем новое
устройство с именем ESP32test и подключим его.
Рис. 2.18. Выбор примера SerialToSerialBT
Глава 2
40
Для дальнейшей работы нам потребуется программа для смартфона по управлению роботами по Bluetooth. Пусть это будет Arduino Bluetooth RC Car — ищем ее
на Google Play и устанавливаем (рис. 2.19).
Запускаем программу, подключаемся к устройству ESP32test, открываем в
Arduino IDE Монитор порта и начинаем нажимать экранные кнопки Bluetooth RC
Car. В Мониторе порта мы видим повторение символов, которые посылает смартфон на ESP32 по Bluetooth (рис. 2.20). Если у робота будут колеса или другие подвижные элементы, можно определять, какие символы приходят, и запускать на
роботе ответные действия — например, робот может ехать и поворачивать.
Рис. 2.19. Окно программы Arduino Bluetooth RC Car на смартфоне
Рис. 2.20. Монитор порта: символы, отправляемые со смартфона на ESP32 по Bluetooth
*
*
*
В данной главе мы получили минимальные навыки работы с ESP32 в среде
Arduino IDE. В главе 3 мы расширим наши знания, выберем моторы для нашего
будущего робота и научимся ими управлять.
ГЛАВА 3
ВЫБОР ДВИГАТЕЛЕЙ, ТЕСТЫ
ПРОГРАММНОГО УПРАВЛЕНИЯ
ШАГОВЫМИ МОТОРАМИ
Моторы коллекторные, бесколлекторные,
шаговые и сервомоторы
ESP32 может поддерживать все решения по подключению моторов (двигателей), которые доступны проектам на Arduino:
• коллекторные моторы;
• бесколлекторные моторы;
• шаговые моторы;
• сервомоторы.
Рассмотрим все эти устройства подробнее.
Коллекторные моторы
Коллекторные моторы — это наиболее распространенный тип двигателей для
сборки ходовой части роботов. Используя понижающие редукторы, от них
можно добиться достаточной мощности для организации движения робота.
Недостаток коллекторных моторов заключается в требующихся для их работы
коллекторных щетках, которые находятся на статоре и замыкаются на обмотки
ротора, — эти щетки постепенно снашиваются и выходят из строя. Кроме того,
вследствие прерывистого контакта щеток с обмотками происходит искрение,
вызывающее электромагнитные помехи. В книге «Мобильные роботы на базе
Arduino» мы решали эту проблему установкой на клеммах моторов фильтрующих конденсаторов. Коллекторные моторы отличаются простотой управления:
для организации вращения вала двигателя в одну сторону достаточно одного
транзистора или реле, при вращении вала в обе стороны обычно применяется
специальный драйвер.
42
Глава 3
Бесколлекторные моторы
Обмотки бесколлекторных моторов неподвижно закреплены на статоре, так что
в них нет щеток и, соответственно, искрения. Да и КПД их выше, чем у коллекторных моторов. Поэтому они представляют собой хороший выбор, если вы
делаете робота, рассчитанного на длительную эксплуатацию. Бесколлекторные
моторы нуждаются в более сложной схеме управления — им для вращения требуется переменное напряжение на обмотках, которое должен уметь создавать
драйвер. Как правило, бесколлекторные моторы имеют высокие обороты и эксплуатируются с понижающими редукторами. Подобные моторы можно встретить в квадрокоптерах.
Шаговые моторы
Шаговые моторы представляют собой модификацию бесколлекторных моторов
с двумя обмотками (преимущественно). Особенностью этого типа моторов является способность их управляемо перемещать вал в точные позиции без использования обратной связи от датчиков углового положения. Перемещение между рядом находящимися позициями удержания называется шагом, количество подобных позиций на оборот зависит от конструкции обмоток и системы управления.
Заметим, что в больших мобильных роботах использование шаговых моторов
в качестве ходовых не практикуется, поскольку они тяжелые и потребляют большой ток даже при простое без нагрузки (постоянно находятся под напряжением).
Сервомоторы
Сервомотор, или сервопривод — это мотор любого типа из только что рассмотренных, в котором реализована функция обратной связи по положению ротора
и коррекция положения ротора согласно информации о текущем реальном его
положении и командам, поступающим на драйвер. Можно самостоятельно сделать сервопривод из коллекторного мотора с редуктором, добавив к мотору датчик оборотов или датчик положения ротора. Примером сервопривода являются
сервомашинки для создания поворотных механизмов в роботах.
Обоснование выбора шагового мотора
Во всех главах книги задействована одна и та же базовая конструкции робота.
В одной из глав мы научим робота запоминать и воспроизводить пройденный
путь, в другой — балансировать на двух колесах. Такие проекты можно реализовать только на моторах точного позиционирования, к которым относятся
шаговые моторы и сервомоторы. Конечно, сервомоторы во многом превосходят своих шаговых собратьев: они экономичнее, а наличие обратной связи от
вала мотора дает им возможность иметь точную информацию о перемещении.
Выбор двигателей, тесты программного управления шаговыми моторами
43
Однако использование шаговых моторов является одним из самых простых, дешевых и легких решений для реализации систем точного позиционирования.
Они проще в программировании и, как правило, дешевле. А так как мы создаем учебные проекты, применение в них хорошо настроенных шаговых моторов
вполне оправданно. Кроме того, шаговые моторы широко применяются в системах позиционирования станков, так что материал книги вполне может помочь
разобраться и в подобных проектах.
Основы работы шаговых моторов
Принцип работы
Шаговый мотор представляет собой
синхронный бесщеточный (бесколлекторный) электродвигатель с несколькими обмотками (рис. 3.1). Подача на
разные его обмотки тока различного
направления и величины приводит к
появлению электромагнитного поля,
которое воздействует на противоположно намагниченные полюса ротора,
заставляя его перемещаться в заданное
положение. Управление подобными
двигателями относительно непростое,
и обычно этим занимается отдельное
устройство — драйвер шагового мотора.
Рис. 3.1. Шаговый двигатель со снятой
нижней крышкой
П ояс не н ие
Обмотки статора (см. рис. 3.1) намагничиваются после подачи на них тока определенной величины и направления. Ротор же представляет собой вал с насаженным на него
намагниченным неодимовым постоянным магнитом. Намагниченный током обмоток
статора имеет участки, которые отталкиваются от также намагниченных полюсов
(участков) ротора, и имеет участки, которые притягиваются к противоположно намагниченным полюсам ротора. Ротор стремится перейти в состояние, в котором участки
статора и ротора взаимно притягиваются, возникает сила, направленная в сторону состояния притяжения, и ротор поворачивается на необходимую величину. Затем направление и величина тока обмоток статора изменяется на противоположные (эту
операцию как раз производит драйвер, выполняя команду «сделать шаг»). Те участки,
которые только что притягивались друг к другу, начинают отталкиваться, и ротор «перепрыгивает» в следующее положение взаимного притяжения электрически намагниченных участков статора и полюсов магнита ротора. Так как ток в обмотках статора
не может изменяться мгновенно (мешает индуктивное сопротивление), требуется время, чтобы перемагнитить статор, а также совершить работу по перемещению ротора
в «комфортное положение», и это время зависит от нагрузки на ротор.
Глава 3
44
Выбор конкретной модели
Крутящий момент, — это сила, которую способен развивать мотор.
4,5±0,1
Кр у т я щ и й моме н т
Ø22-8.052
Конструкций шаговых моторов много, но мы остановимся на моторе, имеющем
200 дискретных состояний на один оборот своего вала, — подобное перемещение (шаг) соответствует повороту на 1,8°. Его мощность должна быть достаточной для вращения колеса под нагрузкой, которую создает робот при движении.
В нашем случае следует выбрать двигатель с активным сопротивлением обмоток 2–3 Ом и номинальным током обмоток не менее 1,2 А. Под эти требования подходит, к примеру, двигатель
23,5±0,5
38 MAX
17HS4401 (рис. 3.2) с крутящим моментом 420 mN•m, углом шага 1,8° и
18±0,2
током фазы 1,5 А.
9,4
2±0,2
42,3 MAX
31±0,1
42,3 MAX
31±0,1
4–М3
Deep: 4.5 MIN
PH–06AWJ
а
–0.003
Ø5 –0.008
123456
A BC D
16,2
б
Рис. 3.2. Шаговый мотор 17HS4401 (а) и его размеры (б)
A
M
C
B
D
Рис. 3.3. Шаговый двигатель (схема)
Схематично шаговый мотор изображают в виде круга с буквой «М» внутри и двумя обмотками (рис. 3.3). На
деле обмоток больше (см. рис. 3.1), но
подключены они только к двум управляющим источникам.
Выбор двигателей, тесты программного управления шаговыми моторами
45
Драйвер DRV8825 и особенности его использования
Драйвер шагового двигателя управляет токами: изменяет направление и величину токов обмоток, создавая электромагнитное поле, требуемое для перемещения
магнитного ротора в следующее строго определенное положение. Движение ротора при этом не плавное, а дискретное — подобные дискретные состояния, как
мы уже отмечали, называются шагами.
Рассмотрим распространенный драйвер для управления шаговыми моторами на основе микросхемы DRV8825.
Он располагается на плате с 16 контактами (рис. 3.4).
Названия ряда контактов сверху имеют линию — это знак «инверсии» сигнала, он означает, что активный сигнал на этой линии — отрицательный:
ENABLE
VMOT
M0
GND MOT
M1
B2
M2
B1
RESET
A1
SLEEP
A2
STEP
DIR
FAULT
GND LOGIC
• ENABLE — включение управлеРис. 3.4. Драйвер шагового мотора DRV8825
ния мотором (если 1 — выключен,
на плате
0 — включен);
• М0, М1, М2 — задают режим деления (дробления) шага (см. табл. 3.1);
• RESET — перезагрузка: в рабочем состоянии мотора должен быть подтянут
к питанию логики;
• SLEEP — сон: в рабочем состоянии мотора должен быть подтянут к питанию логики;
• STEP — шаг: импульс на этом входе выдает команду мотору сделать шаг;
• DIR — задает направление вращения;
• VMOT — контакт для подачи положительного (+) напряжения питания;
• GND MOT — контакт для подачи отрицательного (GND) напряжения питания;
• B1 и B2 — разъемы для подключения первой обмотки;
• A1 и A2 — разъемы для подключения второй обмотки (номера обмоток
и номера контактов в обмотках условны);
• FAULT — выход сигнала аварии;
• GND LOGIC — «земля» логической части (если источники питания логики
и силовой части различны и не имеют общей «земли»).
Некоторые современные драйверы шаговых моторов могут дробить шаги на
более мелкие за счет плавного изменения тока обмоток. Наш драйвер DRV8825
может делить шаг на 32 микрошага. Мы воспользуемся этой возможностью для
дробления шага до 1/16 с целью увеличить плавность движения робота, что в нашем случае важно.
Глава 3
46
На плате драйвера имеется подстроечный резистор для регулировки тока
фазы (он не изолированный и находится под небольшим напряжением).
Производитель рекомендует производить настройку следующим образом:
1. Подключить драйвер к питанию.
2. Измерить напряжение (в вольтах) между стальной головкой резистора и
GND («землей»).
3. Умножить полученное значение на 2 и получить ток фазы в амперах.
Чтобы получить ток фазы 2 А, следует резистором выставить напряжение 1 В.
Чтобы получить ток фазы 1,4 А, следует резистором выставить напряжение
0,7 В. Вращаем головку резистора, измеряем напряжение между ней и GND и
добиваемся требуемого значения.
Большие значения тока фаз приведут к нагреву драйвера и мотора, а также ускорят
разряд батареи робота, меньшие значения могут стать причиной того, что робот
под нагрузкой не сможет вращать колеса или иногда будет «пропускать» шаги.
Таблица 3.1. Значение микрошага шагового мотора в зависимости от подключения
контактов M0, M1 и M2 драйвера DRV8825
M0, вольт
M1, вольт
M2, вольт
Микрошаг
—
—
—
Полный шаг
3,3
—
—
1
—
3,3
—
1
—
1
3,3
1
3,3
1
3,3
—
3,3
3,3
—
—
/2
/4
/8
/16
/32
—
3,3
3,3
1
3,3
3,3
3,3
1
/32
/32
Практическая проверка работы
шагового мотора
Сборка тестовой схемы
Теперь нам нужно получить практические навыки управления шаговым мотором и разобраться в особенностях его разгона и торможения.
Соберем стенд — монтажная и электрическая схемы подключения питания к
связке «шаговый мотор – драйвер» приведены на рис. 3.5 и 3.6 соответственно.
Выбор двигателей, тесты программного управления шаговыми моторами
47
Рис. 3.5. Монтажная схема питания шагового мотора
S
DRV 2
DRV8825
2
3
4
5
6
7
8
+
ENABLE
VMOT
M0
GND1
M1
a1
M2
a2
RESET
b1
SLEEP
b2
STEP
DIRECTION
VDD
GND2
2
2
1
C1
0,1mF
16V
M2
8
7
1
1
1
2
2
14,8–16V
ROB-08420
A
6
5
4
C
3
2
1
Рис. 3.6. Электрическая схема питания шагового мотора
B
D
Глава 3
48
Конденсатор в цепи питания драйвера должен иметь значение емкости не менее
100 микрофарад и рассчитан на напряжение не ниже 16 вольт. Согласно документации, на драйвер DRV8825 должно подаваться напряжение не ниже 9 В (что
связано с применением в драйвере полевых транзисторов), поэтому мы возьмем
4 литиевых аккумулятора 18650, подключим их последовательно и получим от
них 14,8–16 вольт. Также важен ток, который способны отдавать батареи. Ток
рассчитывается исходя из нагрузки и в пиковом режиме может достигать 3 А
(именно на такой суммарный ток мы настроим пару драйверов). Выключатель
также должен быть рассчитан на такой ток. В предыдущем разделе рассказано,
как настраивать ток драйвера при помощи подстроечного резистора на плате.
Монтажная и электрическая схемы управления драйвером DRV8825 от ESP32
показаны на рис. 3.7 и 3.8 соответственно. При этом ESP32 будет получать питание от компьютера по USB. Поскольку источники питания драйвера и ESP32
разные, следует объединить контакты GND и GND LOGIC обеих плат.
Рис. 3.7. Монтажная схема управления драйвером DRV8825 от ESP32
Выбор двигателей, тесты программного управления шаговыми моторами
U1
3V
GND
EN
TX
VP
RX
VN
3V
32
22
33
21
LOLIN32
V1.0.0 GND
34
35
GND
25
19
26
23
27
18
14
5
12
3V
13
17
5V
16
GND
4
0
+Bat
GND
GND
2
15
49
Правый мотор (драйвер)
DRV8825
1
2
3,3 V
3
3,3 V
4
5
6
7
8
8
ENABLE
VMOT
M0
GND1
M1
a1
6
M2
a2
5
RESET
b1
4
SLEEP
b2
3
STEP
DIRECTION
VDD
GND2
7
2
1
Рис. 3.8. Электрическая схема управления драйвером DRV8825 от ESP32
Итак, подключим контакты SLEEP, RESET и M2 драйвера к контакту 3.3v
контроллера, контакт ENABLE драйвера — к контакту 2 GPIO контроллера,
STEP — к 4 GPIO, а DIR — к 16 GPIO. Стенд готов. Драйвер переведен в управление с дроблением шага 1/16, и на один оборот вала мотора нужно будет сделать
3200 шагов (200×16).
Со в е т
Для лучшей визуализации работы мотора стоит приклеить/насадить на вал мотора
«стрелку-метку», по которой будет удобно отслеживать его положение.
Регулировка тока драйвера
Подключим питание драйвера и вращением подстроечного резистора добьемся значения напряжения на нем (на его стальной головке относительно GND)
0,7–0,8 вольт (рис. 3.9).
Глава 3
50
0,8 В
Мультиметр
Питание двигателя
(8,2–45 В)
VMOT
ENABLE
M0
GND
M1
B2
M2
B1
RESET
A1
SLEEP
A2
STEP
FAULT
DIR
100 uF
NEMA 17
GND
DRV8825
Рис. 3.9. Измерение напряжения на подстроечном резисторе мультиметром
В н и ма н ие !
Контролируйте напряжение мультиметром и не превышайте рекомендованный уровень 0,7–0,8 вольт. Если этого не сделать, то большие значения тока через обмотки
двигателя приведут к его разогреву, начнет также греться сам драйвер, да и аккумуляторы разрядятся значительно быстрее, а КПД от завышенного тока не повысится. Не
допускайте перегрева и выхода из строя компонентов!
Со в е т
Заметим, что токи достигают наибольшего значения при простое, когда реактивное
сопротивление обмоток статора току минимально, поэтому рекомендуем в режиме
простоя отключать обмотки мотора, что и будет реализовано в программе.
Программа управления
Программа должна осуществлять вращение вала:
• в выбранную сторону;
• на заданное количество шагов;
• задавать время между шагами с целью добиться плавной устойчивой работы двигателя.
Выбор двигателей, тесты программного управления шаговыми моторами
51
Программа генерирует последовательность импульсов на контакте GPIO 4 контроллера (контакт STEP драйвера). Каждый импульс дает мотору команду сделать один шаг. Последовательность импульсов заставит вал нашего мотора вращаться. Предварительно на GPIO 2 (ENABLE) нужно подать низкий уровень
сигнала, после чего драйвер зафиксирует вал мотора, подав ток на обмотки. Из
практики: драйверу требуется небольшое время (0,1 сек) после получения сигнала ENABLE, чтобы включиться и зафиксировать положение ротора в устойчивом состоянии, иначе первые сгенерированные импульсы будут пропущены.
Что будет подано на GPIO 16 (DIR) — высокий или низкий сигнал, определяющий направление вращения, — нам пока безразлично.
Самый простой способ генерации импульсов — это последовательное переключение сигнала GPIO 4 (STEP) с низкого на высокий уровень (рис. 3.10), при этом
сигнал должен оставаться в определенном уровне заданное время. Сам сигнал
шага не должен быть длительным — достаточно 10 микросекунд, чтобы драйвер его распознал как команду сделать шаг. Полная длительность шага задается
временем шага плюс время низкого сигнала до следующего шага — именно в это
время должен уложиться мотор, чтобы закончить перемещение в следующую
позицию. Если этого не произойдет, двигатель начнет гудеть и не стартует.
0
delayMicrosec(10);
10 мкс
digitalWrite (4, false);
3.3
digitalWrite (4, true);
V (напряжение на GPIO4)
Вольт
delayMicrosec(290);
290 мкс
t, мкс
Рис. 3.10. Генерация шага на GPIO 4 (сигнал STEP)
Проще всего реализовать команды, задающие шаги для мотора в цикле, но контроллеры ESP не жалуют длительные циклы, — контроллер может счесть их
зависанием и выполнит перезагрузку. Поэтому мы сделаем иначе. Пусть будет
произведен шаг и зафиксировано время начала шага, а программа продолжит
исполняться. После начала следующего вызова функции loop(), а она вызывается сразу после своего завершения, произведем опрос: не подошло ли время
сделать следующий шаг. Если нет — пропускаем этот блок, иначе — повторяем
шаг и запоминаем время начала шага. Упрощенный алгоритм работы программы приведен на рис. 3.11. Двигатель делает 600 шагов и в дальнейшем до перезагрузки контроллера простаивает.
Глава 3
52
Начало
MAXi = 600 // Количество шагов
microsec_steplong = 3000 // Длительность шага 8 мкс
Если
сделано
менее MAXi
шагов
Нет
Конец
Нет
Да
Если
подошло время
следующего
шага
Да
Сделать шаг
Рис. 3.11. Упрощенный алгоритм работы программы (см. listing_3_1.ino)
Листинг 3.1. Запуск шагового мотора
#define EN 2 // GPIO ESP32 На пин драйвера включение - удержание.
#define DIR 16 // GPIO ESP32 На пин драйвера - направление вращения.
#define STEP 4 // GPIO ESP32 На пин драйвера - управление шагами.
int i = 0; // Текущий номер шага.
int MAXi = 600; // Количество шагов.
int microsec_steplong = 3000; // Длительность шага в мкс.
long timer;
void setup() {
pinMode(EN, OUTPUT);
pinMode(DIR, OUTPUT);
pinMode(STEP, OUTPUT);
digitalWrite(EN, 1); // Пока двигатель отключен.
digitalWrite(STEP, 0);
Выбор двигателей, тесты программного управления шаговыми моторами
53
digitalWrite(DIR, 1);
timer = micros() + microsec_steplong;
digitalWrite(EN, 0); // Двигатель включен.
Serial.println("BEGIN");
}
void loop()
{
long timeT;
timeT = micros(); // Текущее время.
if (timeT >= timer)
{
if (i < MAXi)
{
// Здесь хранится время начала следующего шага.
timer += microsec_steplong;
digitalWrite(STEP, 1);
delayMicroseconds(10); // Держим сигнал шага поднятым 10 мкс.
digitalWrite(STEP, 0);
i++;
}
else if (i == MAXi)
{
Serial.println("motor stoped");
delay(500); // Гасим инерцию движения.
digitalWrite(EN, 1); // Отключаем мотор.
Serial.println("END");
i++;
}
}
}
Рассмотрим детально программу управления (ее первая версия представлена
в листинге 3.1). В строках, начинающихся с #define, задаются три константы,
соответствующие номерам GPIO управления драйвером. Время между шагами
задается переменной microsec_steplong в микросекундах.
Не забываем перевести GPIO 4, 16 и 2 в состояние вывода:
pinMode(номер GPIO, OUTPUT);
Теперь загрузим программу в ESP32 и подключим питание шагового мотора.
Нажмем на ESP32 кнопку RESET — двигатель сделает 600 шагов и остановится.
Глава 3
54
Задание 1
При установленном делении шага 1/16 поворот оси должен составлять:
600 шагов × 360°/(200 шагов × 16) = 67,5°
Повторите прогон программы несколько раз и убедитесь в правильности ее работы, затем измените количество шагов в программе (переменная MAXi) и проведите повторные измерения. Убедитесь, что работа мотора изменилась.
Измерьте угол, на который повернулся вал мотора, — он должен точно соответствовать количеству поданных шагов.
Со в е т
Измерение можно производить транспортиром или использовать повороты на
360 градусов:
1. Подсчитать, сколько шагов требуется на полный оборот вала (в нашем случае
200×16).
2. Записать в переменную MAXi программы значение, кратное величине, рассчитанной в предыдущем пункте.
3. Провести тестирование — метка на валу, используемая в качестве ориентира, должна сделать выставленное количество оборотов и прийти точно в позицию, которую
занимала перед началом теста.
Задание 2
Изменяя программный код, уменьшите, а затем увеличьте значение длительности шага: microsec_steplong. Скорость вращения вала должна измениться.
При определенной уменьшаемой величине microsec_steplong перемещение
мотора перестанет соответствовать ожиданию: мотор не будет успевать реагировать на команды, и поворот вала мотора перестанет соответствовать количеству заданных шагов), а при дальнейшем уменьшении длительности шага мотор
перестанет стартовать и начнет гудеть. Это значит, что команды шагов достигли
скорости, при которой старт двигателя не происходит: двигатель еще не успевает
провернуться на заданный угол, как уже приходит новая команда на следующий
шаг, и двигатель сбивается, перестает плавно и устойчиво вращаться. Если увеличить напряжение питания мотора, то пороговое значение microsec_steplong
можно будет несколько уменьшить, но полностью избавиться от проблемы не
удастся. Можно лишь помочь стартовать двигателю, раскрутив вал рукой.
Выводы
• Мы научились запускать шаговый мотор.
• Скорость его вращения зависит от длительности шага (времени, выделенного на шаг).
Выбор двигателей, тесты программного управления шаговыми моторами
55
• Шаговый мотор передает перемещение с высокой точностью, что показали
измерения.
• Несоблюдение скоростного режима (длительности шага) может приводить
к пропуску шагов и остановке мотора.
Конечно, пропуск шагов в системах точного позиционирования недопустим. Но
в реальной программе шаговый мотор постоянно должен будет менять свою скорость и направление в зависимости от ситуации. Поэтому нам нужно научиться
гибко управлять шаговым мотором. И начнем мы с добавления возможности
перезапуска, а также рассмотрим возможность разгона мотора.
Разгон/торможение, перезапуск
Перед нами стоит задача: составить программу, осуществляющую разгон и торможение мотора, а также добавить возможность повторного запуска (перезапуска) мотора без перезагрузки контроллера. Перезапуск осуществлять от компьютера, для управления следует использовать последовательный порт, по которому стенд (контроллер ESP32) подсоединен.
Выполняя задания предыдущего раздела, мы убедились, что выход за рабочие
пределы шагового мотора вполне реален. Эффекта остановки мотора на большой частоте управляющих импульсов можно избежать, если ускорять двигатель
постепенно. Тем самым порог максимальной скорости будет значительно отодвинут. Давайте обдумаем, как реализовать подобную функцию.
Разгон в формулах
Последовательность генерируемых программой импульсов должна быть похожа на график, показанный на рис. 3.12, — вначале импульсы идут редко, потом
паузы между ними начинают сокращаться. Достигнув нужной скорости, частота
V (напряжение на GPIO4)
Вольт
3,3
0
Ускорение
Высокая
скорость
Торможение
Остановка
Рис. 3.12. Диаграмма генерации шагов при ускорении/торможении шагового мотора
t
Глава 3
56
импульсов становится постоянной, а в конце участка осуществляется обратная
операция — отрицательное ускорение (увеличиваем паузы между импульсами).
При этом общее количество импульсов должно быть равно заданному в задаче
количеству.
Рассмотрим, как можно реализовать ускорение шагового мотора. Введем понятия:
• скорость начала отчета: speed0. Это скорость (шаг/сек), на которой двигатель может стартовать из состояния покоя или начать изменять свою скорость, получив новое ускорение;
• время от начала отчета: dt;
• длительность стартового шага: max_steplong. Эта величина обратно пропорциональна стартовой скорости (сек, микросек);
• максимальная скорость: speedMax. Это скорость, до которой можно разогнать двигатель (шаг/сек);
• длительность минимального шага: min_steplong Эта величина обратно
пропорциональна максимальной скорости (сек, микросек);
• ускорение мотора: acceleration. Это ускорение, при котором мотор без
сбоев может разогнаться до максимальной скорости (шаг/сек2);
• текущее ускорение: accelerationT. Это ускорение, с которым мотор двигается в текущий момент;
• длительность текущего шага: msec_steplong. Это время, выделенное на
шаг, рассчитанное и примененное к мотору.
Задание: сделать 6000 шагов
Двигатель должен стартовать со скоростью speed0, ускориться до скорости
speedMax, затем, сделав 6000 шагов, он должен быть остановлен, причем предварительно его скорость должна быть снижена до стартовой скорости speed0 —
для исключения проскакивания заданного значения в 6000 шагов по инерции.
Так как мы имеем шаговый двигатель, то удобнее оперировать не скоростью,
а длительностью шага. Рассчитаем значения длительности начального/конечного шага (здесь длительность шага представлена в микросекундах):
max_steplong = 1000000/speed0
Теперь сделаем то же для длительности шага при максимальной скорости
(в микросекундах):
min_steplong = 1000000/speedMax
Рассчитаем величину текущего шага, который будет следующим:
msec_steplong = 1000000 / (speed0 + (dt* accelerationT)/1000000)
Выбор двигателей, тесты программного управления шаговыми моторами
57
Величина текущего шага msec_steplong потребуется в микросекундах и рассчитывается в микросекундах. Для этого в числителе присутствует число 1000000 —
количество микросекунд в секунде.
Определим, какими типами чисел следует пользоваться, чтобы не исказились
вычисления. Если использовать 16-разрядные числа (имеющие 16 двоичных разрядов), то возможные значения ограничены величинами от –32768 до
+32767, — судя по формулам, этого не достаточно. Нужно использовать 32- или
64-разрядные переменные, для этого подходят типы данных long и int32_t, хранящие значения от –2,147,483,648 до 2,147,483,647.
Алгоритм работы программы
Нам нужно разогнать двигатель, затем двигаться без ускорения, потом равноускоренно затормозить до стартовой скорости (рис. 3.13).
Пусть переменная Stage хранит этап работы мотора:
• Stage = 1 — разгоняем мотор;
• Stage = 2 — мотор вращается без ускорения;
• Stage = 3 — тормозим мотор;
• Stage = 5 — мотор остановлен, ждем новых команд.
В начале работы программы Stage = 5 — мотор простаивает, шаги не формируются, проверяется наличие на порту Serial любого байта.
После поступления на порт Serial байта, Stage переводим в состояние 1, задаем начальную скорость speed0 (от нее будем рассчитывать текущую скорость),
запоминаем время начала работы timerT, находим половину от числа требуемых шагов mediana (может потребоваться, если максимальная скорость еще не
достигнута, а уже пора тормозить).
Генерируя шаги, одновременно проверяем, достигнута ли максимальная скорость, и если она достигнута, то переводим режим в Stage = 2 — движение
без ускорения, запоминаем, сколько шагов нам потребовалось для ускорения,
вернее, нам потребуется шаг, на котором пора начать торможение: braking.
Параметру accelerationT присвоим значение 0, и если максимальная скорость
не достигнута, но количество шагов достигло половины от заданного, то переводим режим в Stage = 3.
Затем движемся без ускорения, пока не достигнем количества шагов, после которого нужно начать торможение braking. Достигнув значения шагов braking,
значению текущего ускорения accelerationT присвоим значение – (минус)
Глава 3
58
Начало
1
MAXi = 6000; //Количество шагов.
min_steplong = 200; // Длительность после разгона шага в мкс.
max_steplong = 3000; // Длительность начального шага в мкс.
acceleration = 10000; //Ускорение в шагах на секунду^2.
stage = 5 ( двигатель простаивает, готов принимать команды)
Если stage = 5
(Шаги закончились,
двигатель
простаивает)
Нет
Нет
timerT>=timer
Если шаг закончен
Нет
Получить текущее
время,
timerT = micros();
1
Пришел символ
на Serial-порт
Да
Да
Получить текущее время.
timerT = micros();
Получить время с начала работы цикла:
dt = timerT- start_time;
Рассчитать длину шага на основании ускорения/
торможения и времени от начала движения,
msec_steplong = 1000000 / (speed0 +
+ (dt* accelerationT)/1000000);
Время окончания шага:
timer = timerT +msec_steplong;
Запомнить время начала работы мотора,
start_time = micros();
Рассчитать начальную скорость
speed0 = 1000000.0 / double(max_steplong);
Найти половину от заданного количества шагов:
mediana = MAXi / 2;
Задать ускорение: accelerationT = acceleration;
Сбросить счетчик шагов: i = 0;
Установить флаг — Двигатель раскручивается:
stag = 1;
Сделать шаг,
Генерируется 10 мкс импульс на GPIO 4 (Step)
Увеличиваем счетчик шагов: i ++;
Да
Нет
Нет
Сделано MAXi
шагов
Stage = 1
Фаза разгона
Нет
1
1
Stage = 2
Фаза постоянной
скорости
Нет
Достигли
максимальной
скорости
msec_steplong<=
min_steplong
Сделали
половину шагов
I >=mediana
Да
Все шаги сделаны,
больше шагов не будет
Stage = 5;
1
1
Да
Да
2
Да
Начинаем торможение,
Stage = 3;
speed0 = 1000000/msec_steplong;
accelerationT = - acceleration;
timerT = tImer;
Да
i>=braking,
Шаги на торможение
Да
2
Ускорение завершено,
accelerationT = 0;
Далее, до начала торможения двигаемся
без ускорения, Stage = 2;
Рассчитываем номер шага, на котором нужно
начать торможение, braking = MAXi – I;
speed0 = 1000000/msec_steplong;
timerT = tImer;
1
Рис. 3.13. Алгоритм работы программы (см. listing_3_2.ino)
Выбор двигателей, тесты программного управления шаговыми моторами
59
acceleration, переводим режим в Stage = 2, speed0, приравниваем к текущей
скорости, время начала работы timerT присваиваем к текущему времени.
Далее только генерируем шаги с отрицательным ускорением и проверяем, достигло ли количество шагов значения, заданного MAXi.
Программа (листинг 3.2) допускает перезапуск цикла вращения путем подачи
управляющего символа из Монитора порта (по Serial-порту), при этом двигатель ускоряется и тормозит равноускоренно, что позволяет существенно повысить максимальную скорость.
Листинг 3.2. Вращение с ускорением
#define EN 2 // GPIO ESP32 На пин драйвера включение - удержание.
#define DIR 16 // GPIO ESP32 На пин драйвера - направление вращения.
#define STEP 4 // GPIO ESP32 На пин драйвера - управление шагами.
int i = 0; // Текущий номер шага.
int64_t MAXi = 6000; // Количество шагов.
int64_t min_steplong = 200; // Длительность после разгона шага в мкс.
int64_t max_steplong = 3000; // Длительность начального шага в мкс.
int64_t msec_steplong;
int64_t start_time;
int8_t stag;
double acceleration = 10000; // Ускорение в шагах на секунду^2.
double accelerationT; // Текущее ускорение
double speed0; // Стартовая скорость.
double dt;// Приращение времени от начала ускорения/торможения.
int64_t timer;
int64_t steps_to_stop; // Количество шагов, требуемое для торможения.
int64_t mediana; // Середина цикла
void setup()
{
pinMode(EN, OUTPUT);
pinMode(DIR, OUTPUT);
pinMode(STEP, OUTPUT);
digitalWrite(EN, 1); // Пока двигатель отключен.
digitalWrite(STEP, 0);
digitalWrite(DIR, 1);
msec_steplong = max_steplong;
Глава 3
60
accelerationT = acceleration;
speed0 = 1000000.0 / double(max_steplong);
stag = 5;
Serial.begin(115200);
Serial.println("Boot");
Serial.println("Wait command from UART");
}
void loop()
{
if (stag == 5) // Двигатель не работает
{
if (Serial.available()) // Если что-то есть на входе
{
Serial.end();// На время движения откл. UART
digitalWrite(EN, 0); // Двигатель включен.
delay(100); // Время на включение драйвера.
stag = 1;
steps_to_stop = 0;
speed0 = 1000000.0 / double(max_steplong);
start_time = micros();
i = 0;
mediana = MAXi / 2;
timer = micros(); // Текущее время.
}
}
long timeT = micros(); // Текущее время.
if ((timeT >= timer) && (stag != 5))
{
i++;
switch (stag) {
case 1:
// Разгон
// Считаем количество шагов на разгон/торможение.
steps_to_stop++;
digitalWrite(STEP, 1); // Поднимаем шаг
dt = double(timeT - start_time) * 0.000001; // Переводим в секунды
msec_steplong = long(1000000.0 / (speed0 + acceleration * dt));
// msec_steplong = max_steplong - acceleration * dt;
if (msec_steplong <= min_steplong)
Выбор двигателей, тесты программного управления шаговыми моторами
61
{
msec_steplong = min_steplong; // Ограничиваем скорость.
stag = 2; // Переходим в фазу поступательного движения
// Изменяем значение стартовой скорости.
speed0 = 1000000.0 / msec_steplong;
}
else
//Если не достигли макс.скорости, но уже половина пути...
if (steps_to_stop >= mediana)
{
stag = 3; // Переходим в фазу торможения.
// Изменяем значение стартовой скорости.
speed0 = 1000000.0 / msec_steplong;
start_time = timeT;
}
delayMicroseconds(10); // Держим сигнал шага поднятым 10 мкс.
digitalWrite(STEP, 0);
break;
case 2: // Поступательное движение - без ускорения.
digitalWrite(STEP, 1); // Поднимаем шаг
delayMicroseconds(10); // Держим сигнал шага поднятым 10 мкс.
digitalWrite(STEP, 0);
// Если оставшиеся шаги равны тормозному пути....
if ((MAXi - i) <= steps_to_stop)
{
stag = 3;
start_time = timeT;
}
break;
case 3: // Торможение.
digitalWrite(STEP, 1); // Поднимаем шаг
dt = double(timeT - start_time) * 0.000001; // Переводим в секунды
msec_steplong = long(1000000.0 / (speed0 - acceleration * dt));
// Если рассчитанный шаг больше, чем достаточный для остановки.
if (msec_steplong >= max_steplong)
{
msec_steplong = max_steplong; // Ограничиваем скорость.
}
Глава 3
62
delayMicroseconds(10); // Держим сигнал шага поднятым 10 мкс.
digitalWrite(STEP, 0);
if (i >= MAXi) // Если все шаги сделаны.
{
stag = 4; // Остановка.
// Удерживаем вал в течение этого времени.
msec_steplong = 500000;
}
break;
case 4: // Отключение мотора.
digitalWrite(EN, 1);
stag = 5;
Serial.begin(115200);
break;
}
// Здесь хранится время начала следующего шага.
timer += msec_steplong;
}
}
Загружаем программу, открываем Монитор порта, устанавливаем скорость порта 115 200, подключаем питание к нашему стенду, в Мониторе порта нажимаем
кнопку ввода (клавишу <Enter> клавиатуры). Шаговый мотор должен стартовать, раскрутиться, затем некоторое время вращаться на высокой скорости, далее постепенно затормозить и остановиться. Повторное нажатие кнопки ввода
приведет к повторению процесса.
Задание
Поэкспериментируйте с изменением ускорения acceleration. Поэкспериментируйте с величиной максимальной скорости, изменяя минимальную длительность шага min_steplong.
Выводы
Если двигатель будет увеличивать скорость постепенно, избегая резких рывков, максимальная скорость вращения ротора значительно возрастет. Есть
предел максимальной скорости, после достижения которой двигатель теряет
управляемость. Теперь мы умеем более гибко обращаться с шаговым мотором
и научились превышать стартовую скорость за счет постепенного раскручивания мотора.
Выбор двигателей, тесты программного управления шаговыми моторами
63
Внешнее интерактивное управление скоростью,
количеством и направлением шагов, ускорением
При использовании шаговых моторов по назначению, скорость, направление
и ускорение должны изменяться динамически, поэтому требуется, чтобы программа поддерживала ввод этих значений. Добавим в программу управление
максимальной скоростью, количеством шагов, направлением вращения и ускорением, а чтобы было удобнее исследовать двигатель, создадим модуль ввода
данных по Serial-интерфейсу.
Основная программа останется прежней, фактически уже сейчас ничего не мешает изменять указанные параметры, но нужно определиться, как это сделать
извне программы. Поэтому мы первым делом разработаем несложный протокол обмена, который позволит четко интерпретировать, какой параметр пришел и как его обработать.
При этом должна быть обеспечена возможность передавать определенную
буквенно-цифровую последовательность. Для этого следует в среде Arduino IDE
открыть Монитор порта и по номеру порта, к которому подключен контроллер
ESP32 стенда, получить и передать через него строку ввода данных. Программа,
работающая на контроллере стенда, будет принимать эту строку, понимать и использовать.
Пусть команда приходит в виде символьно-цифровой последовательности
SxxMrrAnnE, где вместо xx — вводится максимальная скорость в шагах на секунду, вместо rr — значение шагов, которые нужно сделать (при этом допустимо
делать отрицательные шаги), вместо nn — значение ускорения в шагах на секунду в квадрате, символ E — завершает ввод данных. Если какой-либо из параметров не введен до получения символа E, этот параметр берется из ранее установленных значений для указанных переменных. Отправка данных происходит по
нажатию на клавиатуре ПК клавиши <Enter>.
Рассмотрим пример: S3000M-6000A3000E означает, что двигатель должен раскрутиться до скорости 3000 шагов в секунду, при этом сделать 6000 шагов, вращаясь «условно» против часовой стрелки, использовать для разгона ускорение
3000 шагов/сек2.
Для ввода данных используется функция bool data_fromUART(), которая читает информацию с Serial-порта, и в зависимости от поступившего ключевого символа (S, M, A) заполняет один из трех массивов chisloS[], chisloM[]
и chisloA[] приходящими цифрами. Затем, когда приходит символ E, означающий конец пакета, заполненные массивы конвертируются в числа, обозначающие: максимальную скорость, число и направление шагов, ускорение.
Глава 3
64
В листинге 3.3 приведен только участок программы, где вызывается функция
data_fromUART(), а также сама функция data_fromUART().
Листинг 3.3. Вращение по вводимым данным
#define EN 2 // GPIO ESP32 На пин драйвера включение - удержание.
#define DIR 16 // GPIO ESP32 На пин драйвера - направление вращения.
#define STEP 4 // GPIO ESP32 На пин драйвера - управление шагами.
int i = 0; // Текущий номер шага.
. . .
void setup()
{
. . .
Serial.println("Boot");
Serial.println("Wait command from UART");
}
void loop()
{
if (stag == 5) // Двигатель не работает
{
if (Serial.available()) // Если что-то есть на входе
{
if (data_fromUART())
{
//Serial.end();// На время движения откл. UART
....
}
....
}
char chisloM[10], chisloS[10], chisloA[10];
int indexS = 0, indexA = 0, indexM = 0;
char inpDATA;
bool end_input = false;
bool data_fromUART()
{
//sDATAaDATAdData
while (Serial.available()) // Принимаю число
Выбор двигателей, тесты программного управления шаговыми моторами
{
char b = Serial.read();
// Начало ввода параметра
// S - скорость.
// М - перемещение, кол.шагов.
// A - ускорение
if ((b == 'S') || (b == 'M') ||
{
inpDATA = b;
continue;
}
else if (b == 'E') // Окончание
{
end_input = true;
break;
}
if (((b == '-')) || (b == '1')
|| (b == '2') || (b == '3')
|| (b == '5') || (b == '6')
|| (b == '8') || (b == '9')
{
switch (inpDATA) {
case 'S':
if (indexS < 8)
{
chisloS[indexS] = b;
indexS++;
}
break;
case 'M':
if (indexM < 8)
{
chisloM[indexM] = b;
indexM++;
}
break;
case 'A':
if (indexA < 8)
{
chisloA[indexA] = b;
indexA++;
(b == 'A'))
ввода
|| (b == '4')
|| (b == '7')
|| (b == '0'))
65
Глава 3
66
}
break;
}
}
}
if (end_input)
{
if (indexM > 0)
{
MAXi = atol(&chisloM[0]); // Из строки в число
for (indexM = 0; indexM < 8; indexM++)
chisloM[indexM] = '\0';
indexM = 0;
Serial.print("MAXi="); Serial.println(int(MAXi));
if (MAXi < 0) {
MAXi = -MAXi;
digitalWrite(DIR, 0);
}
else digitalWrite(DIR, 1);
}
if (indexS > 0)
{
double Speed = double(abs(atol(&chisloS[0]))); // Из строки в число
for (indexS = 0; indexS < 8; indexS++)
chisloS[indexS] = '\0';
indexS = 0;
Serial.print("Speed="); Serial.println(Speed);
min_steplong = int64_t(1000000.0 / Speed);
}
if (indexA > 0)
{
acceleration = double(abs(atol(&chisloA[0]))); // Из строки в число
for (indexA = 0; indexA < 8; indexA++)
chisloA[indexA] = '\0';
indexA = 0;
Serial.print("Acceleration="); Serial.println(acceleration);
}
end_input = false;
return true; // Ввод осуществлен
}
return false; // Ввода не было
}
Выбор двигателей, тесты программного управления шаговыми моторами
67
Задание 1
Загрузите измененную программу в контроллер — полный ее скетч (listing_3_3.
ino) вы найдете в файловом архиве, сопровождающем книгу (см. приложение).
Проведите ряд экспериментов, составьте график: при каких значениях ускорения стартует и разгоняется двигатель, и какой максимальной скорости удалось
достичь.
Задание 2
Наденьте на вал шагового мотора какой-нибудь тяжелый предмет (маховик).
Двигатель также нужно закрепить — например, в струбцине или в тисках.
Повторите эксперимент по разгону. На практике с увеличением массы раскручиваемой области разрешенные значения ускорения должны снижаться — убедитесь в этом сами. Инерция робота, которого мы будем делать, достаточно
велика, поэтому значения ускорения на практике нужно будет значительно
уменьшить.
Выводы
Значение разрешенного ускорения зависит от массы ротора. При увеличении
массы ротора разгоняться и тормозить двигатель нужно медленнее, выбирая
меньшие значения ускорения.
Со в е т
Если что-то не получается, не расстраивайтесь, отдохните, обдумайте, где вы могли
ошибиться. В файловом архиве, сопровождающем книгу, имеются все приведенные в
книге программы (скетчи). Если вы не уверены, что правильно написали программу
по листингам из книги, попробуйте взять готовую из архива, загрузить ее в контроллер и проверить работу. Проверьте также электрические соединения, а если и в этом
случае работа не получается, обратитесь к моему каналу на YouTube: www.youtube.
com/c/MikhailMomot, я нередко обсуждаю там работу шаговых моторов. В любом
случае не отступайте и не сдавайтесь!
*
*
*
После успешного проведения всех тестов можно приступать к сборке базовой
модели робота, чему и посвящена следующая глава.
ГЛАВА 4
СОБИРАЕМ БАЗОВУЮ МОДЕЛЬ
Настало время собрать базовую модель мобильного робота под управлением
контроллера ESP32 с ходовой частью на базе шаговых моторов.
Сборка механики робота
Соберем механическую часть робота (его корпус и моторы) и обсудим особенности конструирования робота с использованием шаговых моторов.
Конструкция и крепление шаговых моторов
Шаговые моторы могут иметь различные габариты, но соответствуют определенным стандартам по размеру вала и по расстоянию между крепежными винтами на передней стороне корпуса. В этой главе мы будем использовать моторы
с размерами, совпадающими с приведенными на рис. 4.1. Наша задача — надежно закрепить двигатели на корпусе робота, а также обеспечить жесткость
корпуса, так как если корпус получится недостаточно жестким, это приведет
к деформациям конструкции, и работоспособность робота окажется под вопросом (на рис. 4.5 показан пример деформации колесной пары под весом шаговых
двигателей).
Вариант рамки для крепления моторов приведен на рис. 4.2. Рамка состоит из
связывающей панели и двух крепежных, которые имеют по 4 отверстия для крепления мотора. Рамку можно выгнуть из металла или склеить из фанеры, можно также сделать ее из пластика или распечатать на 3D-принтере. Важно при
этом выдержать расстояние между отверстиями для крепления моторов и расстояние между моторами. Моторы крепятся к рамке винтами М3 длиной 6–8 мм
(рис. 4.3). На оси моторов надеваются колеса (рис. 4.4).
Как уже упоминалось ранее, если материал рамки окажется недостаточно жестким, конструкция может деформироваться (см. рис. 4.5). В этом случае нужно
Глава 4
70
23,5±0,5
4–М3
Deep: 4.5 MIN
38 MAX
42,3 MAX
31±0,1
42,3 MAX
31±0,1
4,5±0,1
Ø22-8.052
18±0,2
9,4
2±0,2
PH–06AWJ
–0.003
Ø5 –0.008
123456
A BC D
16,2
Рис. 4.1. Размеры мотора Nema 17 17HS4401
Отверстия под винты
крепления моторов
Рис. 4.2. Крепежная рамка из фанеры для
крепления пары моторов Nema 17 17HS4401
Рис. 4.3. Мотор Nema 17 17HS4401 закреплен
четырьмя винтами
Рис. 4.4. Моторы Nema 17 17HS4401, рамка и колеса
Собираем базовую модель
Рис. 4.5. Пример деформации рамки крепления моторов
71
Рис. 4.6. Стальной уголок
для крепления шагового
мотора к корпусу робота
применить более жесткий материал или сделать дополнительные ребра жесткости. К примеру, можно использовать даже стальной уголок (рис. 4.6) — он
точно не будет деформироваться, хотя и прибавит нашему роботу немного лишней массы.
В нашем же проекте мы выполним рамку из фанеры толщиной 3 мм и применим
дополнительные связывающие элементы.
Ф а й ло в ы й а рх ив
Чертежи всех деталей корпуса робота в электронном виде доступны читателям книги в
папке model robot сопровождающего книгу файлового архива (см. приложение). Детали
можно распечатать на 3D-принтере или вырезать из фанеры.
Требования к колесам
Колеса по диаметру должны быть не менее 63 мм — в противном случае робот будет скрести корпусом по полу, особенно при неровной поверхности. На
рис. 4.4 колеса имеют диаметр обода 73 мм, и еще 3–4 мм добавляет резиновая
покрышка.
Можно изготовить колеса следующими способами:
• нарезать 4–5 заготовок из фанеры и склеить их между собой, чтобы ширина
колеса (не менее 10 мм) позволяла прочно зафиксировать вал мотора в центральном отверстии (5 мм). Для устранения прокручивания вала в фанере
следует при сборке капнуть на вал капельку «суперклея» (этилцианкрилат);
• фрезеровать колеса из толстого пластика (капролона) — колеса получаются
очень прочные, но это более дорогой вариант;
Глава 4
72
• напечатать колеса на 3D-принтере, тут можно придумать еще и оригинальный дизайн, а колеса получаются прочными и легкими. Можно печатать их
в один слой из пластика PETG;
• просто купить готовые колеса диаметром от 63 мм и более с центральным
отверстием 5 мм.
Со в е т
Для колес нужны резиновые покрышки — отлично подходит пористая резина для
обмотки руля спортивного велосипеда (рис. 4.7). Она приклеивается к ободу на клей
«Момент». Если клеить аккуратно, получается очень симпатичное колесо, особенно
если использовать разноцветную резину.
Иногда можно найти в продаже покрышки без колес, в этом случае можно приобрести их, измерить, а затем смоделировать и распечатать обод на 3D-принтере.
13 мм
25 мм
35 мм
Рис. 4.7. Резина для обмотки руля велосипеда
Рис. 4.8. Третье — поворотное — колесо
для робота
Рис. 4.9. Три колеса робота (вид без корпуса)
Собираем базовую модель
73
Наша базовая модель будет иметь два ведущих колеса и маленькое поворотное
колесо (рис. 4.8). Подобные колеса имеют в основании шариковый подшипник,
снижающий трение при повороте/развороте. Колесо крепится к корпусу четырьмя винтами М3. Расположение всех колес показано на рис. 4.9.
З а ме ча н ие
Подобная конструкция специально задумана как трансформер. В последней главе мы
снимем третье колесо, поставим робота вертикально и научим балансировать и двигаться на двух колесах!
Поворотное колесо может быть задним или передним — оба варианта имеют
плюсы и минусы, но мы будем считать, что оно расположено сзади.
Сборка корпуса
Корпус робота может быть любым и из любого доступного материала, кроме картона, даже папье-маше допустимо. Но если конструкция гнется, нужно
взять другой материал или установить дополнительные фиксирующие ребра.
Базовая конструкция проста и универсальна. Она применима во всех
рассмотренных далее проектах мобильных роботов, включая и модель двухколесного балансирующего робота.
Порядок сборки корпуса робота
приведен на рис. 4.10–4.14. Винты
и места их установки (см. рис. 4.3),
чтобы не загромождать изображение, здесь не показаны. Поворотное
колесо и аккумуляторные боксы
также крепятся на винты. Палуба
робота съемная, все остальные соединения можно склеить при помощи «суперклея», термопистолетом
или другим прочным клеем.
Рис. 4.10. Сборка борта робота
Глава 4
74
Рис. 4.11. Сборка корпуса робота
Собираем базовую модель
75
Рис. 4.12. Крепление аккумуляторных боксов
Рис. 4.13. Сборка палубы робота
Глава 4
76
Рис. 4.14. Корпус робота в сборе
Сборка электроники
Монтаж электронной части робота, как правило, делают двумя способами.
Самый надежный — пайка. Самый простой и доступный — беспаечный монтаж
с использованием проводов с разъемами типа dupont и макетной платы.
Особенности установки контроллера ESP32
на макетную плату
Если плата контроллера ESP32 шире, чем ваша макетная плата, она перекроет
полностью отверстия с одной из сторон макетной платы, и не будет возможности подключить туда соединительные провода. На рис. 4.15 видно, что с каждой
стороны платы есть по одному ряду отверстий для проводов, а вот на рис. 4.16
нижний ряд отверстий макетной платы полностью перекрыт платой контроллера. Выходом может быть отказ от монтажа платы ESP32 на макетную плату
и непосредственное подключение проводов к контактным штырькам контроллера. Второй вариант — использовать две платы для монтажа (рис. 4.17). Вся
остальная электроника может быть смонтирована на макетных платах без особых проблем.
Собираем базовую модель
77
Рис. 4.15. Плата контроллера LOLIN32 на макетной плате: с каждой стороны платы контроллера
есть по одному ряду отверстий для подключения проводов
Рис. 4.16. Плата контроллера ESP32 на макетной плате: нижний ряд отверстий макетной платы
перекрыт платой контроллера, и к нему невозможно что-либо подключить
Глава 4
78
Рис. 4.17. Плата контроллера ESP32 установлена на две макетные платы
Далее изображения монтажа на макетных платах не приводятся, так как это
ухудшает читаемость схем, но подразумевается, что их можно применять.
Со в е т
Если вы не знаете, как работать с макетными платами, пройдите по ссылке: https://
youtu.be/soFmO9TNBiA и посмотрите приведенный там материал. Или введите
в строке поиска YouTube макетная плата и посмотрите другие обучающие видео.
Сборка схемы электропитания
Начнем собирать схему с силовой части. Монтажная и электрическая принципиальная схемы питания электромоторов приведены на рис. 4.18 и 4.19 соответственно. Четыре аккумулятора соединены последовательно для получения
напряжения, достаточного для гарантированного срабатывания шаговых моторов. В цепи питания драйверов установлены конденсаторы (100 мкФ, 16 В) для
сглаживания бросков тока (подобная схема уже рассматривалась в главе 3). Не
забываем проверить фазы мультиметром: a1, a2 — одна фаза, b1, b2 — вторая.
Собираем базовую модель
79
Подключаем драйверы, подаем питание и настраиваем максимальный ток фазы
на 1,5 А при помощи подстроечного резистора (напряжение на нем должно быть
порядка 0,7–0,8 вольт).
Монтажная и электрическая принципиальная схемы питания контроллера
ESP32 приведены на рис. 4.20 и 4.21 соответственно.
В главе 3 подразумевалось, что стенд будет использоваться в соединении с компьютером, поэтому внешнее питание на ESP32 там не подавалось (питание
Рис. 4.18. Монтажная схема электропитания шаговых моторов
Глава 4
80
ESP32 производилось по кабелю USB). Однако наш робот становится автономным, поэтому на модуль ESP32 требуется подать напряжение 5 вольт — для этого
можно применить линейный или импульсный стабилизатор. Импульсный стабилизатор экономичнее, линейный — надежнее. Мы воспользуемся линейным
стабилизатором L7805, а для отвода тепла (если стабилизатор будет сильно
греться) рекомендуется прикрепить к нему небольшой радиатор. Конденсатор
на входе должен быть рассчитан на напряжение не ниже 16 вольт, емкость конденсаторов от 0,33 мкФ или более.
S
+
DRV 2
DRV8825
3
4
5
6
7
8
M0
GND1
M1
a1
M2
a2
RESET
b1
SLEEP
b2
STEP
DIRECTION
VDD
GND2
2
3
4
5
6
7
8
VMOT
M0
GND1
M1
a1
M2
a2
RESET
b1
SLEEP
b2
STEP
DIRECTION
VDD
GND2
1
M2
7
ROB-08420
A
6
5
4
C
3
2
1
B
+
ENABLE
2
8
DRV 2
DRV8825
1
2
C1
0,1mF
16V
1
VMOT
2
2
ENABLE
D
C1
0,1mF
16V
M1
8
7
1
1
1
2
2
ROB-08420
A
6
5
4
3
C
2
1
B
D
Рис. 4.19. Электрическая принципиальная схема электропитания шаговых моторов
Собираем базовую модель
81
Рекомендуется поставить на вход 5V контроллера ESP32 диод (в разрыв от выхода стабилизатора L7805 к контакту 5V ESP32) для устранения обратных токов при программировании контроллера через компьютер по USB.
Рис. 4.20. Монтажная схема электропитания ESP32
1
2
2
1
2
2
C2
0,1mF
16V
2
3
C3
0,1mF
16V
7805
GND
OUT
1
1
D1
1N5817 +
IC2
IN
1
+
2
GND
3V
TX
EN
RX
VP
VN
3V
22
32
21 LOLIN32
33
34
GND V1.0.0
GND
35
19
25
23
26
18
27
5
14
3V
12
17
13
16
5V
4
GND
0
GND
+Bat
2
GND
15
10–16V
S2
Рис. 4.21. Электрическая принципиальная схема электропитания контроллера ESP32
Глава 4
82
Схема управления
Монтажная и электрическая принципиальная схемы подключения управления
моторами приведены на рис. 4.22 и 4.23 соответственно. Включением моторов
управляет контакт GPIO 2 ESP32. Управляющий сигнал приходит на контакты
ENABLE обоих драйверов. Учтите, что контакт ENABLE — инверсный, и подача
на него положительного сигнала: DigitaWrite(2, true) — отключит моторы,
а отрицательного: DigitaWrite(2, false) — включит, и вал моторов зафиксируется до подачи шаговых импульсов.
Шаговые импульсы для левого мотора от GPIO 15 приходят на контакт STP левого драйвера, а для правого от GPIO 4 — на контакт STP правого драйвера.
По положительному фронту импульса моторы будут делать шаг величиной, заданной перемычками M0–M2 (см. табл. 3.1). В нашем случае, согласно схеме,
показанной на рис. 4.23, — это микрошаг 1/16 от полного шага (1,8°).
Направлением вращения моторов управляют GPIO 13 (приходит на контакт DIR
на драйвере левого мотора) и GPIO 16 (приходит на контакт DIR на драйвере
Рис. 4.22. Монтажная схема управления ESP32 шаговыми моторами
Собираем базовую модель
83
Правый мотор (драйвер)
DRV8825
U1
3V
GND
EN
TX
VP
RX
VN
3V
32
22
33
21
LOLIN32
V1.0.0 GND
34
35
GND
25
19
26
23
27
18
14
5
12
3V
13
17
5V
16
GND
4
0
+Bat
GND
GND
2
15
1
2
3,3 V
3
3,3 V
4
5
6
7
VMOT
M0
GND1
M1
a1
6
M2
a2
5
RESET
b1
4
SLEEP
b2
3
STEP
8
8
ENABLE
DIRECTION
VDD
GND2
7
2
1
Левый мотор (драйвер)
DRV8825
1
2
3
3,3 V
4
5
6
7
8
8
ENABLE
VMOT
M0
GND1
M1
a1
6
M2
a2
5
RESET
b1
4
SLEEP
b2
3
STEP
DIRECTION
VDD
GND2
7
2
1
Рис. 4.23. Электрическая принципиальная схема управления ESP32 шаговыми моторами
правого мотора). Отметим, что направление вращения зависит от сигнала на
том или ином GPIO и порядка подключения фаз мотора к драйверу. Если вращение происходит не в планируемую сторону, можно инвертировать значение
сигнала в программе, подавая низкие сигналы вместо высоких. Можно также
поменять полярность подключения одной из фаз мотора к драйверу.
Тестовая программа
В главе 3 мы рассмотрели программу управления вращением шагового мотора,
в которой шаговые импульсы генерируются в основной программе. Если строить по той же схеме более сложные программы управления, то они окажутся
слишком громоздкими и трудночитаемыми. Поэтому удобнее и нагляднее использовать другие возможности контроллера ESP32, а именно прерывания по
таймеру.
Глава 4
84
Работа с прерываниями
Прежде чем писать «большую» базовую программу управления роботом, изучим работу прерываний на небольшой тестовой программе.
Перед нами стоит задача:
• изучить, как работают прерывания;
• используя прерывания, активировать шаговые моторы;
• вызывая прерывания, генерировать импульсы step для драйверов шаговых
моторов с заданной изменяемой частотой.
Механизм прерываний дает возможность задать событие, результатом которого
будет запуск определенной функции, привязанной к этому событию. События
могут быть различными — например, появление высокого/низкого уровня сигнала на определенном выводе GPIO или срабатывание аппаратного таймера
контроллера. Аппаратный таймер похож на будильник, который вы заводите на
определенное время, — такой «будильник» может срабатывать периодически
или только один раз. В наших проектах мы будем ориентироваться на функцию периодического срабатывания. Когда будильник-таймер достигает времени срабатывания, запускается функция, закрепленная в программе в качестве
обработчика прерывания, при этом основная программа на время обработки
прерывания останавливается и после завершения обработки прерывания продолжается с места остановки.
П р е и м у ще с тв о п р оце ссора с д в ум я яд ра м и
Основная программа может и не прерываться, если функция обработки прерывания
выполняется на втором ядре, а у нас их два!
Основное требование к программам обработки прерываний — это краткость,
они не должны потреблять много ресурсов процессора и существенно влиять на
скорость работы основной программы, в них должно быть минимум кода и не
должно быть задержек типа delay() или delayMicroseconds(). Кроме того,
излишне частый вызов прерываний по таймеру также уменьшает производительность программы.
В стандартных примерах Arduino IDE для ESP32 есть программа организации
работы с прерываниями RepeatTimer (рис. 4.24).
Рассмотрим детально, что требуется для создания прерывания и установки обработчика события прерывания по таймеру:
Собираем базовую модель
85
Рис. 4.24. Выбор примера программы работы с таймером RepeatTimer
1. Создать указатель на прерывание: hw_timer_t * timer = NULL.
2. Объявить и описать функцию, которая вызывается по срабатыванию прерывания: void IRAM_ATTR onTimer(), — в ней и должны быть описаны все
те действия, которые нужно производить периодически.
3. Создать прерывание по таймеру: timer = timerBegin(0, 80, true).
Первый параметр — это номер используемого таймера (их всего 6). Второй
параметр — предделитель, на эту величину предварительно делится частота тактового генератора для таймеров1. Теперь на наш таймер будут приходить импульсы с частотой 1 МГц — каждую микросекунду. Третий параметр
указывает, будет таймер работать на увеличение (true) или уменьшение
(false) значения.
4. Прикрепить функцию, которая объявлена в п. 2, к прерыванию в качестве
обработчика: timerAttachInterrupt(timer, &onTimer, true).
5. Задать,
через
какой
промежуток
времени
сработает
таймер:
timerAlarmWrite(timer, 1000000, true), — в нашем случае это одна секунда2. Третий параметр — это периодичность: если он установлен в false,
то прерывание сработает только 1 раз, иначе — периодически.
6. Срабатывание
прерывания можно включить вызовом функции:
timerAlarmEnable(timer). Отключение прерывания — функцией:
timerAlarmDisable(timer) в любом месте программы. Также можно изменять периодичность срабатывания — например, по команде timerAlarm
1
В ESP32 для таймеров генерируется частота 80 МГц.
Частота генератора тактовых импульсов для таймера сначала делится на 80 предделителем
(см. п. 3) — теперь таймер тактируется частотой 1 МГц. То есть срабатывание таймера происходит
через 1 000 000 тактов, и при периодическом срабатывании это соответствует частоте в 1 Гц (1 раз
в секунду).
2
Глава 4
86
Write(timer, 100, true). Прерывание при этом будет вызываться через
каждые 100 тактов таймера — при установленном предделителе 80, 100 тактов таймера соответствуют 100 микросекундам, то есть функция обработки
прерывания будет вызываться с частотой 10 000 Гц (10 000 раз в секунду).
Изменим стандартный пример, добавив в него параметры управления моторами робота. Пусть импульсы шага генерируются не в основной программе, а по
прерыванию. Рассмотрим изменения стандартного примера:
1. Добавлены директивы компилятору по замене выражений: STEP_L, STEP_R
(GPIO управления шагами), STEPER_EN (GPIO включения обоих драйверов), DIR_L, DIR_R (GPIO направления вращения) — на номера следующих
GPIO:
#define STEP_L 15
#define STEP_R 4
#define STEPER_EN 2
#define DIR_L 13
#define DIR_R 16
2. Добавлены булевы (принимающие только два значения: true или false)
переменные: STEP_L_level (уровень сигнала на GPIO генерации шага левого мотора), STEP_R_level (уровень сигнала на GPIO генерации шага правого мотора), DIR_L_level (уровень сигнала на GPIO направления вращения
левого мотора), DIR_R_level (уровень сигнала на GPIO направления вращения правого мотора), STEPER_EN_level (текущее состояние GPIO включения моторов: true — моторы выключены, false — моторы включены):
bool STEP_L_level = false;
bool STEP_R_level = false;
bool DIR_L_level = false;
bool DIR_R_level = false;
bool STEPER_EN_level = true;
3. Функция обработки прерывания изменена — она инвертирует текущее значение уровня шага (STEP_L_level, STEP_R_level) и записывает его в соответствующий GPIO (STEP_L и STEP_R):
void IRAM_ATTR onTimer()
{
STEP_L_level = !STEP_L_level;
digitalWrite(STEP_L, STEP_L_level);
STEP_R_level = !STEP_R_level;
digitalWrite(STEP_R, STEP_R_level);
}
Собираем базовую модель
87
4. В функции setup() добавлены команды по переводу GPIO в состояние вывода данных:
void setup() {
pinMode(STEP_L, OUTPUT);
pinMode(STEP_R, OUTPUT);
pinMode(DIR_L, OUTPUT);
pinMode(DIR_R, OUTPUT);
pinMode(STEPER_EN, OUTPUT);
. . .
5. В функции loop() никаких действий не производится, но остается возможность их туда вставить, — шаги на моторы будут генерироваться в фоновом
режиме по прерыванию.
Произведенные изменения сведены в листинг 4.1. Шаги генерируются с половиной от частоты прерывания — 50 раз в секунду (за два вызова генерируется
полный импульс), это маленькая скорость, но достаточная для тестирования,
и робот не сбежит ;-). Результатом работы программы должно быть вращение
обоих колес робота с одинаковой скоростью. Направление вращения пока не
имеет значения.
Листинг 4.1. Тест вращения колес робота
#define STEP_L 15 // Пин левого шага
#define STEP_R 4 // Пин правого шага
#define STEPER_EN 2 // Пин включения моторов
#define DIR_L 13 // Пин направления левого шага
#define DIR_R 16 // Пин направления правого шага
bool STEP_L_level = false;
bool STEP_R_level = false;
bool DIR_L_level = false;
bool DIR_R_level = false;
bool STEPER_EN_level = true;
// Ссылка на таймер.
hw_timer_t * timer = NULL;
// Функция обработки прерывания по таймеру.
void IRAM_ATTR onTimer()
{
88
Глава 4
STEP_L_level = !STEP_L_level;
digitalWrite(STEP_L, STEP_L_level);
STEP_R_level = !STEP_R_level;
digitalWrite(STEP_R, STEP_R_level);
}
//==============
void setup() {
pinMode(STEP_L, OUTPUT);
pinMode(STEP_R, OUTPUT);
pinMode(DIR_L, OUTPUT);
pinMode(DIR_R, OUTPUT);
pinMode(STEPER_EN, OUTPUT);
// Начальное значение уровня сигнала
// на шаговых контактах.
STEP_L_level = false;
STEP_R_level = false;
// Задаем направление вращения.
DIR_L_level = false;
DIR_R_level = false;
// Активируем моторы.
STEPER_EN_level = false;
digitalWrite(STEP_L, STEP_L_level);
digitalWrite(STEP_R, STEP_R_level);
digitalWrite(DIR_L, DIR_L_level);
digitalWrite(DIR_R, DIR_R_level);
digitalWrite(STEPER_EN, STEPER_EN_level);
Serial.begin(115200);
// Создаем таймер с предделителем 80,
// что для нашего контроллера дает отсчет 1 раз в микросекунду.
timer = timerBegin(0, 80, true);
// Прикрепляем функцию onTimer в качестве обработчика прерывания.
timerAttachInterrupt(timer, &onTimer, true);
// Для таймера timer создаем "будильник",
// срабатывающий через 100 000 отчетов.
// Последний параметр true - периодичное срабатывание.
Собираем базовую модель
89
// false - разовое срабатывание.
timerAlarmWrite(timer, 100000, true);
// Активируем будильник.
timerAlarmEnable(timer);
}
//==============
void loop() {
// Ничего не делаем.
delay(1000);
}
Задание
Самостоятельно измените программу так, чтобы скорость моторов возросла
в 2 раза. Какие изменения для этого требуются?
Базовая программа управления роботом
Перед нами стоит задача: составить базовую программу, в которой будут проработаны основные функциональные модули управления движением робота, и
выполнить ее тестирование с применением доступного механизма управления
роботом.
О со б е н но с т и по с т р о е н и я б ол ь ш и х п р ог ра м м
Среда Arduino IDE, в которой мы составляем программы, не является столь продвинутым редактором кода, как, например, Visual Studio Code от Microsoft. Это весьма простой редактор, и править в нем сложные длинные программы не очень удобно: можно
легко запутаться в длинном тексте, сложно найти нужный участок кода. Поэтому для
удобства редактирования рекомендуется большие программы разбивать на несколько
отдельных файлов, содержащих участки кода по определенным направлениям и имеющих имена, которые ассоциируются с их содержимым. Такие файлы должны иметь
расширение имени h. Для того чтобы объединить несколько подобных файлов в одну
программу, используется объявление #include "имя файла" — по этой инструкции
при подготовке кода вашей программы к записи в контроллер ESP32 (компиляции),
содержимое указанного файла просто вставляется в заданное место. Получается длинный текстовый файл с программой, и уже этот файл транслируется в коды процессора
и передается в его энергонезависимую память.
Определение моторов в программе (файл motorstep.h)
Монтажная и электрическая принципиальная схемы подключения управления
моторами были показаны на рис. 4.22 и 4.23 соответственно.
Глава 4
90
В файле motorstep.h (листинг 4.2.1) мы определим основные команды, которые
будут применяться для организации движения робота.
Так, в листинге 4.2.1 определены контакты GPIO подключения драйверов к контроллеру (имена констант и номера GPIO задаются директивами #define):
• STEP_L и STEP_R — GPIO левого и правого шагов;
• STEPER_EN — GPIO включения моторов;
• DIR_L и DIR_R — GPIO направления левого и правого шагов.
П ри ме ча н ие
Если вы используете другие GPIO, вам следует учесть это при проработке материала
этого раздела.
Направление вращения моторов определяют четыре константы: DIR_L_FORWARD,
DIR_L_BACKWARD, DIR_R_FORWARD и DIR_R_BACKWARD. Их значения задаются
в результате тестирования робота. Например, если вы дадите команду роботу
ехать вперед, а левое колесо начнет крутиться назад, следует поменять местами
значения, присвоенные DIR_L_FORWARD, DIR_L_BACKWARD.
Со в е т
Можно также не вносить изменения в программу, а поменять местами контакты, подключенные к A1 и A2 или к B1 и B2 (для драйвера и мотора, который имеет неверное
направление вращения).
Переменная maxSPEED служит для хранения значения максимальной скорости робота (шаг*100/сек), до которой ему позволено разгонять свои моторы.
Значение maxSPEED может быть изменено. Переменная minSPEED задает минимальную скорость (в таком же масштабе), с которой возможен старт мотора
и его остановка без потери шагов. Значение minSPEED не может быть равным
нулю — его следует определять экспериментально (в программе это 100 000 шагов в 100 секунд).
П ояс не н ие
Подобный масштаб скоростей обоснован тем, что нам приходится иметь дело с целыми
4-байтовыми числами, и если не следить за значением переменной, можно получить
значение меньше 1, оно будет «округлено» до нуля, что неверно. Также можно выйти
за верхний диапазон 4-байтовых чисел и получить непредсказуемый результат.
Представим и прочие переменные и функции программы:
• acceleration — базовое ускорение в шагах на секунду в квадрате (поделенное на 1000). Служит для установки ускорений правого и левого моторов;
Собираем базовую модель
91
• accelerationL — рабочее ускорение левого мотора. Может принимать значения: +acceleration, -acceleration и 0;
• accelerationR — рабочее ускорение правого мотора. Может принимать
значения: +acceleration, -acceleration и 0;
• speed_last_L — рабочая максимальная скорость левого мотора, до которой стремится разогнаться мотор в настоящее время. При описании переменной используется ключевое слово volatile, дающее указание компилятору не оптимизировать использование этой переменной (не хранить ее
значение в регистрах), поскольку переменная speed_last_L используется и
изменяется в функции обработки прерывания и одновременно анализируется в основной программе;
• speed_last_R — рабочая максимальная скорость правого мотора, до которой стремится разогнаться мотор в настоящее время;
• speed_left — новая максимальная скорость левого мотора, полученная в
результате прихода внешней команды. В середине текущего шага программа проанализирует ее значение и в случае необходимости изменит скоростные параметры для вычисления скорости мотора;
• speed_right — новая максимальная скорость правого мотора, полученная
в результате прихода внешней команды;
• STEPER_EN_level — булева (значения true или false) переменная для
хранения информации о том, активированы или нет драйверы моторов;
• функция: void setup_motor_system() — вызывается один раз в начале
программы. Она переводит GPIO управления драйверами моторов в состояние вывода, используя оператор pinMode(номер GPIO, состояние),
присваивает им начальные значения (true или false), используя оператор
digitalWrite(номер GPIO, значение);
• функция: void motor_off() — проверяет, включены ли моторы, и если
включены, то отключает их, функция: void motor_on() — выполняет обратную операцию.
Листинг 4.2.1. Файл motorstep.h
#define STEP_L 15 // Пин левого шага
#define STEP_R 4 // Пин правого шага
#define STEPER_EN 2 // Пин включения моторов
// Функция инициализации управления моторами.//
#define DIR_L 13 // Пин направления левого шага
#define DIR_R 16 // Пин направления правого шага
#define DIR_L_FORWARD false
#define DIR_L_BACKWARD true
Глава 4
92
#define DIR_R_FORWARD true
#define DIR_R_BACKWARD false
int32_t maxSPEED = 800000; // Скорость шагов за 100 сек
int32_t minSPEED = 100000; // Скорость шагов за 100 сек
// 20000; теперь это значение поделено на 1000 для вхождения в рамки
// 32-разрядных целых чисел
int32_t
acceleration = 20;
int32_t
accelerationL = 0;
int32_t
accelerationR = 0;
volatile int32_t speed_last_L = 0;
volatile int32_t speed_last_R = 0;
volatile int32_t speed_left = 0;
volatile int32_t speed_right = 0;
//=========================================
//=========================================
bool STEPER_EN_level = true;
int timer_stop; // для отключения двигателей при простое
//===============================
void setup_motor_system()
{
STEPER_EN_level = true;
pinMode(STEP_L, OUTPUT);
pinMode(STEP_R, OUTPUT);
pinMode(DIR_L, OUTPUT);
pinMode(DIR_R, OUTPUT);
pinMode(STEPER_EN, OUTPUT);
digitalWrite(DIR_L, DIR_L_FORWARD);
digitalWrite(DIR_R, DIR_R_FORWARD);
digitalWrite(STEPER_EN, STEPER_EN_level);
}
//===============================
// == Включение /отключение моторов
void motor_off()
{
// Отключаем все колеса.
Собираем базовую модель
93
if (!STEPER_EN_level)
{
STEPER_EN_level = true;
digitalWrite(STEPER_EN, STEPER_EN_level);
}
}
//===============================
void motor_on()
{
// Включаем все колеса.
if (STEPER_EN_level)
{
STEPER_EN_level = false;
digitalWrite(STEPER_EN, STEPER_EN_level);
// Задержка на включение драйверов.
delay(500);
}
}
//======================
Настройка генерации шагов (файл irq_robot.h)
В разд. «Работа с прерываниями» было отмечено, что использование прерываний для генерации командных импульсов шагов моторов удобно и не очень
сложно. При этом требуется, чтобы шаги генерировались с заданными параметрами ускорения.
В главе 3 (см. листинг 3.3) шаги с заданными параметрами ускорения генерировались в основной программе, и расчет длительности шага производился там
же. В реальной программе неэффективно использовать основной цикл loop()
для генерации шагов — экономичнее делать это в фоновом режиме, а в основной программе производить другие работы. Поэтому мы перенесем генерацию
шага и расчет длительности шага в функцию обработчика прерываний (файл
irq_robot.h).
Рассмотрим эту процедуру подробнее (листинг 4.2.2). Пусть все работы по расчету и генерации шагов обоих моторов производятся периодическими вызовами одной функции (по прерыванию). Для создания прерывания объявляем
указатель на таймер:
hw_timer_t * Timer = NULL
Глава 4
94
Переменные
Существенное ускорение работы часто повторяющихся участков программы
дает использование вместо стандартной команды digitalWrite(номер GPIO,
значение) прямой записи значений сразу для обоих моторов в соответствующий регистр вывода GPIO (GPIO_OUT_REG), для чего нужны маски, в которых
прописываются значения устанавливаемых битов в 32-разрядном регистре.
П ояс не н ие
Регистр GPIO_OUT_REG служит для организации записи на контакты GPIO, переведенные в режим вывода (OUTPUT), новых значений одновременно для всех GPIO с номерами от 0 до 31. Но прямое присвоение новых данных регистру не работает — для записи в регистр требуется вызов функции REG_WRITE(GPIO_OUT_REG, NEW_VOLUME), где
NEW_VOLUME — 4-байтовое значение, каждый бит которого несет информацию о новом
значении GPIO с номером, соответствующим разряду бита в NEW_VOLUME.
Рассмотрим эти маски:
• off_all_step — маска для отключения (в значение 0) битов шагов обоих
моторов в регистре вывода GPIO;
• LEFTSTEP_mask_on — маска для включения (в значение 1) бита шагов левого мотора в регистре вывода GPIO;
• RIGHTSTEP_mask_on — маска для включения (в значение 1) бита шагов правого мотора в регистре вывода GPIO.
Для генерации шагов мотора требуется, чтобы такие шаги были предусмотрены (запланированы). С этой целью применяются счетчики шагов LEFT_STEPS_
counter и RIGHT_STEPS_counter. Если переменная содержит положительное
число, то генерируется шаг, и значение переменной уменьшается на 1, после достижения нуля шаги не генерируются.
Рассмотрим также следующие переменные:
• sost — составная маска для записи в регистр состояния GPIO шагов;
• speed0Ltec и speed0Rtec — используются в расчете длительности следующего шага и хранят значение расчетной скорости, при которой и формируется следующий шаг;
• speed0L и speed0R — хранят начальные значения скоростей, используемые
в формуле расчета текущей скорости. Пример для левого мотора:
speed0Rtec = speed0L + accelerationL* dt_L
• DIR_L_tec и DIR_R_tec — служат для хранения состояний направления
вращения моторов;
• newLEFTcounter_do_step и newRIGHTcounter_do_step — хранят значения длительности следующих шагов, которые произойдут после завершения текущего шага;
Собираем базовую модель
95
• LEFTcounter_do_step и RIGHTcounter_do_step — хранят значения длительности текущих шагов;
• dt_L и dt_R — время действия текущего ускорения. Применяются при расчете длительности шага;
• half_LEFT_STEPS и half_RIGHT_STEPS — хранят половину от длительности текущего шага. Когда текущий шаг достигнет середины, производится
расчет параметров следующего шага. Середина шага используется, чтобы
разнести по времени действия, производимые по прерыванию, и снизить
время выполнения функции;
• newhalf_LEFT_STEPS и newhalf_RIGHT_STEPS — то же что и предыдущие,
но для следующего шага;
• LEFTcounter и RIGHTcounter — прошедшее время текущего шага в десятках микросекунд.
Функция void IRAM_ATTR step_timer()
Функция step_timer() представляет собой обработчик прерываний, она вызывается каждые 10 микросекунд, и в ней производятся все периодические расчеты по управлению скоростью моторов.
Сначала выполняется анализ наличия неисполненных шагов для моторов. Если
неисполненные шаги есть, то на основании длительности шага и счетчика шага
формируется очередной шаг. В конце шага формируется положительный импульс на GPIO шага. Длительность импульса равна времени между прерываниями — 10 микросекунд.
Завершает обработку генерации шагов условный вызов команды записи в регистр:
REG_WRITE(GPIO_OUT_REG, значение)
Здесь параметр значение — 32-разрядное число, содержащее биты, номера
которых соответствуют номеру GPIO контроллера. Условный вызов команды
записи в регистр позволяет значительно ускорить запись по сравнению с использованием классической команды digitalWrite(номер GPIO, значение),
которая позволяет изменить только один бит (GPIO) за вызов.
Далее производится проверка того, что текущий шаг (для каждого мотора отдельно) находится в середине:
if (LEFTcounter == half_LEFT_STEPS)
Глава 4
96
Если событие произошло, переходим к расчету длительности следующего шага.
Для этого корректируем время действия текущего ускорения на время сделанного шага:
dt_L += LEFTcounter_do_step;
А затем проверяем, не изменилась ли заданная скорость (она меняется извне):
if (speed_last_L != speed_left)
Если изменение произошло, корректируем ускорение, назначаем новую базовую скорость (она равна speed0Ltec), обнуляем время действия текущего ускорения dt_L (так как начался новый участок, отсчет времени также начинается
сначала).
Далее, имея базовую скорость, ускорение и прошедшее время рассчитываем новую скорость:
speed0Ltec = speed0L
+ accelerationL * dt_L;
Проверяем вхождение полученной скорости в заданные пределы и в случае необходимости корректируем:
speed0Ltec = constrain(speed0Ltec, -maxSPEED, maxSPEED)
Проверяем скорость на достижение минимального предела:
if (abs(speed0Ltec) > minSPEED)
если предел не достигнут, то рассчитываем шаг:
newLEFTcounter_do_step = abs(long(10000000) / speed0Ltec)
Если предел достигнут, анализируется, к какой скорости стремится мотор. Если
к нулю, мотор останавливается, шаги не генерируются. В противном случае длительность следующего шага принимается в 1 миллисекунду (вызовы происходят через 10 микросекунд, значит, присвоив длительности нового шага значение
100, мы получим 10 × 100 = 1000 микросекунд, или 1 миллисекунду):
newLEFTcounter_do_step = 100
Далее условие: if (speed0Ltec < 0) — проверяет, какой знак имеет новая
скорость мотора, и в зависимости от выполнения условия корректируется направление вращения мотора. Если полученная в результате расчетов скорость
имеет отрицательный знак (она меньше нуля), то контакт GPIO, отвечающий
за направление вращения, переключается в значение, которое для этого мотора
приведет к перемещению робота назад (DIR_L_BACKWARD) и наоборот:
digitalWrite(DIR_L, DIR_L_BACKWARD);
DIR_L_tec = DIR_L_BACKWARD;
Собираем базовую модель
97
или
digitalWrite(DIR_L, DIR_L_FORWARD);
DIR_L_tec = DIR_L_FORWARD;
В заключение производится расчет середины длительности следующего шага:
newhalf_LEFT_STEPS = newLEFTcounter_do_step >> 1;
Для положительных целых чисел эта команда аналогична делению на 2, но выполняется значительно быстрее.
Такие же операции проделываются для правого мотора.
Функция void timer_setup()
Функция timer_setup() вызывается единожды в функции setup(). В ней заполняются маски: off_all_step, LEFTSTEP_mask_on и RIGHTSTEP_mask_on на
основании информации о номерах GPIO (STEP_L и STEP_R), используемых для
формирования шагов.
Выполняются настройки GPIO STEP_L и STEP_R — устанавливается низкий уровень сигнала, создается таймер с предделителем 80 (тактирующийся частотой
1 МГц).
К таймеру прикрепляется функция-обработчик step_Timer():
timerAttachInterrupt(Timer, &step_timer, true)
Задается периодичность срабатывания прерывания — каждые 10 микросекунд:
timerAlarmWrite(Timer, 10, true)
Производится старт таймера:
timerAlarmEnable(Timer)
Листинг 4.2.2. Файл irq_robot.h
// Создаем указатель на таймер генерации шагов для моторов робота
hw_timer_t * Timer = NULL;
// Объявляем переменные, хранящие текущее
// состояние уровня сигнала на GPIO шагов для моторов
//=======================================
// Функция, вызываемая по прерыванию –
// генерирует сигнал шага для левого мотора,
// генерирует сигнал шага для правого мотора
Глава 4
98
//+++++++++++++++++++++++++++++++++++++++
// Маска выключения высокого состояния шагов всех моторов
uint32_t off_all_step;
// Маски высокого состояния шагов для каждого мотора
uint32_t LEFTSTEP_mask_on;
uint32_t RIGHTSTEP_mask_on;
// Количество шагов, которые нужно сделать
volatile int
LEFT_STEPS_counter = 0;
volatile int
RIGHT_STEPS_counter = 0;
//================================================
//================================================
uint32_t sost;
// Скорость для расчета длительности следующего шага
int32_t speed0Ltec = 0;
int32_t speed0Rtec = 0;
// Скорость начала расчета
int32_t speed0L = 0;
int32_t speed0R = 0;
// Направление шагов
int32_t DIR_L_tec = DIR_L_FORWARD;
int32_t DIR_R_tec = DIR_R_FORWARD;
// Новое время - длительность шага в десятках мкс
int32_t newLEFTcounter_do_step = 100;
int32_t newRIGHTcounter_do_step = 100;
// Пройденное время в десятках мкс
int32_t dt_L = 0;
int32_t dt_R = 0;
// Середина шага
int32_t half_LEFT_STEPS = 50;
int32_t half_RIGHT_STEPS = 50;
int32_t newhalf_LEFT_STEPS = 50;
int32_t newhalf_RIGHT_STEPS = 50;
// Установленное время - длительность шага в десятках мкс
int32_t
LEFTcounter_do_step = 100;
int32_t
RIGHTcounter_do_step = 100;
Собираем базовую модель
// Прошедшее время текущего шага в десятках мкс
int32_t LEFTcounter = 0;
int32_t RIGHTcounter = 0;
bool old_sost = false;
//++++++++++++++++++++++++++++++++++++++++//
//==== ==//
//++++++++++++++++++++++++++++++++++++++++//
void IRAM_ATTR step_timer() {
// Опустить шаг для всех моторов
// Заменяем множество опускающих команд на
sost = 0;
if (LEFT_STEPS_counter > 0) // Если есть шаги
{
if (LEFTcounter >= LEFTcounter_do_step)
{
sost |= LEFTSTEP_mask_on; // Поднять шаг - пока только флаг
// Счетчик длительности шага - обнуляем для начала нового шага
LEFTcounter = 0;
LEFT_STEPS_counter--; // Счетчик количества шагов – уменьшаем
// Принимаем следующую длительность шага
LEFTcounter_do_step = newLEFTcounter_do_step;
// Устанавливаем новую середину шага
half_LEFT_STEPS = newhalf_LEFT_STEPS;
}
LEFTcounter++; // Счетчик длительности шага
}
if (RIGHT_STEPS_counter > 0 )
{
if (RIGHTcounter >= RIGHTcounter_do_step)
{
sost |= RIGHTSTEP_mask_on; // Поднять шаг - пока только флаг
// Счетчик длительности шага - обнуляем для начала нового шага
RIGHTcounter = 0;
RIGHT_STEPS_counter--;// Счетчик шагов – уменьшаем
// Принимаем следующую длительность шага
RIGHTcounter_do_step = newRIGHTcounter_do_step;
99
Глава 4
100
// Устанавливаем новую середину шага
half_RIGHT_STEPS = newhalf_RIGHT_STEPS;
}
RIGHTcounter++; // Счетчик длительности шага
}
// Если шаг нужно делать
// Опускаем сигналы шагов (пока только в переменной)
// Поднимаем шаги для моторов, которым это нужно
// Записываем значение с измененными битами шагов в регистр GPIO (0-31)
if (sost)
{
old_sost = true;
REG_WRITE(GPIO_OUT_REG,((REG_READ(GPIO_OUT_REG) & off_all_step) | sost));
}
// Если шаги делать не нужно, но нужно опустить
// поднятые при прошлом вызове импульсы шагов
else if (old_sost) {
old_sost = false;
// Опускаем все значения шагов на 0
REG_WRITE(GPIO_OUT_REG, (REG_READ(GPIO_OUT_REG)& off_all_step));
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//================================================================//
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
// Шаг сделан, рассчитываем параметры следующего шага
if (LEFTcounter == half_LEFT_STEPS) // если мы на середине шага
{
// Изменяется значение времени от начала отчета в десятках мкс
dt_L += LEFTcounter_do_step;
// accelerationL ускорение в относительных единицах
// (шагов/сек^2)/1000
//dt_L - количество в десятках мкс с начала действия ускорения.
// speed0L скорость с начала действия данного ускорения
// speed0L - шагов за 100 секунд
// Если пришла новая скорость
if (speed_last_L != speed_left)
Собираем базовую модель
101
{
// Выбираем, какое направление ускорения требуется
// для достижения новой скорости
if (speed0Ltec < speed_left)
{
accelerationL = acceleration;
}
else
{
accelerationL = - acceleration;
}
speed_last_L = speed_left;
// Чтобы стартовать - это длительность максимального шага 0.001 сек.
dt_L = 100;
// Новая базовая скорость задана.
speed0L = speed0Ltec;
}
// Расчет скорости для следующего шага
speed0Ltec = speed0L
+ accelerationL * dt_L;
// Проверка вхождения в пределы и корректировка
speed0Ltec = constrain(speed0Ltec, -maxSPEED, maxSPEED);
// Если полученная скорость больше остановочной
if (abs(speed0Ltec) > minSPEED)
{
// Находим длителность следующего шага
newLEFTcounter_do_step = abs(long(10000000) / speed0Ltec);
// Добавляем 2 в счетчик шагов, иначе не будет новых шагов
LEFT_STEPS_counter = 2;
}
else
{
// Ограничиваем длительность шага
newLEFTcounter_do_step = 100; // или 1000 мкс
// Если скорость стремилась к 0
// Не делаем больше шагов
if (speed_last_L
speed0L = 0;
== 0) {
Глава 4
102
accelerationL = 0;
LEFT_STEPS_counter = 0;
}
else LEFT_STEPS_counter = 2;
}
// Анализируем знак скорости
// При необходимости изменяем направление
// вращения мотора
if (speed0Ltec < 0) {
if (DIR_L_tec == DIR_L_FORWARD)
{
digitalWrite(DIR_L, DIR_L_BACKWARD);
DIR_L_tec = DIR_L_BACKWARD;
}
}
else {
if (DIR_L_tec == DIR_L_BACKWARD)
{
digitalWrite(DIR_L, DIR_L_FORWARD);
DIR_L_tec = DIR_L_FORWARD;
}
}
// Находим середину длительности нового шага
// Для скорости деление заменяем на сдвиг
newhalf_LEFT_STEPS = newLEFTcounter_do_step >> 1;
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//================================================================//
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
// Если мы на середине шага
if (RIGHTcounter == half_RIGHT_STEPS)
{
// Изменяется значение времени от начала отчета в десятках мкс
dt_R += RIGHTcounter_do_step;
// accelerationR ускорение в относительных единицах
// (шагов/сек^2)/1000
// dt_R - количество в десятках мкс с начала действия ускорения.
Собираем базовую модель
103
// speed0R скорость с начала действия данного ускорения
// Если пришла команда изменения скорости
if (speed_last_R != speed_right)
{
if (speed0Rtec < speed_right)
{
accelerationR = acceleration;
}
else
{
accelerationR = - acceleration;
}
speed_last_R = speed_right;
// Чтобы стартовать - это длительность максимального шага 0.001 сек.
dt_R = 100;
speed0R = speed0Rtec;
}
// Расчет скорости для следующего шага
speed0Rtec = speed0R
+ accelerationR * dt_R;
speed0Rtec = constrain(speed0Rtec, -maxSPEED, maxSPEED);
// Если полученная скорость больше остановочной
if (abs(speed0Rtec) > minSPEED)
{
// Находим длительность следующего шага
newRIGHTcounter_do_step = abs(long(10000000) / speed0Rtec);
// Добавляем 2 в счетчик шагов, иначе не будет новых шагов
RIGHT_STEPS_counter = 2;
}
else
{
// Если скорость стремилась к 0
// Не делаем больше шагов
newRIGHTcounter_do_step = 100; // или 1000 мкс
if (speed_last_R
== 0) {
speed0R = 0;
accelerationR = 0;
RIGHT_STEPS_counter = 0;
Глава 4
104
}
else RIGHT_STEPS_counter = 2;
}
//
//
//
if
Анализируем знак скорости
При необходимости изменяем направление
вращения мотора
(speed0Rtec < 0) {
if (DIR_R_tec == DIR_R_FORWARD)
{
digitalWrite(DIR_R, DIR_R_BACKWARD);
DIR_R_tec = DIR_R_BACKWARD;
}
}
else {
if (DIR_R_tec == DIR_R_BACKWARD)
{
digitalWrite(DIR_R, DIR_R_FORWARD);
DIR_R_tec = DIR_R_FORWARD;
}
}
// Находим середину длительности нового шага
// Для скорости деление заменяем на сдвиг
newhalf_RIGHT_STEPS = newRIGHTcounter_do_step >> 1;
}
}
//+++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++//
// Готовим состояния масок
// Создание таймеров, прикрепление к ним
// функций обработчиков прерываний
//+++++++++++++++++++++++++++++++++++++++++++++++//
void timer_setup()
{
off_all_step = 0xFFFFFFFF; //Все биты в 1
// Маски подъема сигналов на GPIO шагов моторов
LEFTSTEP_mask_on = uint32_t(1) << STEP_L;
RIGHTSTEP_mask_on = uint32_t(1) << STEP_R;
Собираем базовую модель
105
off_all_step = off_all_step ^ LEFTSTEP_mask_on;
// Теперь имеем маску, которой можно убрать
// STEP-сигналы сразу с обоих моторов
off_all_step = off_all_step ^ RIGHTSTEP_mask_on;
digitalWrite(STEP_L, false);//STEP_L_level);
digitalWrite(STEP_R, false);//STEP_R_level);
// Создаем таймер
Timer = timerBegin(0, 80, true);
// Прикрепляем функции-обработчики
timerAttachInterrupt(Timer, &step_timer, true);
// Прерывание повторяется каждые 10 микросекунд
timerAlarmWrite(Timer, 10, true);
// Старт таймера, теперь прерывания будут генерироваться
timerAlarmEnable(Timer);
Создание примитивов движений (файл move_case.h)
В главе 3 при рассмотрении работы шагового мотора был сделан важный вывод:
ускорять и тормозить/замедлять шаговый двигатель нужно постепенно (равноускоренно) — тогда можно достичь высокой скорости и избежать перехода мотора в неуправляемый режим. Этого принципа мы будем придерживаться и при
организации движения мобильного робота (листинг 4.2.3).
Функция move_case(char &bt_input)
Функция move_case(char&bt_input) принимает коды команд и изменяет значения скоростей speed_left и speed_right — это новые скорости, к достижению которых будут теперь стремиться моторы. Анализ изменения скоростей
производится в функции, обслуживающей прерывание step_timer() из файла
irq_robot.h (см. листинг 4.2.2). Коды команд и параметры корректировки скорости показаны в табл. 4.1.
Таблица 4.1. Коды команд
Код
команды
F
B
R
Действие в программе
speed_left = maxSPEED;
speed_right = maxSPEED ;
speed_left = - maxSPEED;
speed_right = - maxSPEED;
speed_left
= maxSPEED ;
speed_right = -maxSPEED ;
Описание
Двигаться вперед
Двигаться назад
Поворачивать вправо
Глава 4
106
Таблица 4.1 (окончание)
Код
команды
L
G
I
H
J
S
Действие в программе
speed_left = -maxSPEED;
speed_right = maxSPEED;
speed_left
= 0;
speed_right = maxSPEED;
speed_left
speed_right
speed_left
=
maxSPEED;
= 0;
= - maxSPEED;
speed_right
= 0;
speed_right
= - maxSPEED;
speed_left =0;
speed_left
= 0;
speed_right = 0;
maxSPEED = 300000;
0 – 9,q
….
maxSPEED = 800000;
Описание
Поворачивать влево
Поворачивать влево
при движении вперед
Поворачивать вправо
при движении вперед
Поворачивать влево
при движении назад
Поворачивать вправо
при движении назад
Остановка
Изменение значения максимальной возможной скорости
(шаг×100/сек)
Из данных табл. 4.1 видно, что скоростям моторов присваиваются точные значения, но эти значения не будут сразу применены — скорость робота плавно
начнет изменяться в сторону указанных скоростей с заданным в программе
(acceleration = 20;) ускорением.
Применение переменной flagTimeOff
Переменная flagTimeOff применяется для отслеживания случаев длительного
простоя робота. Ее сравнение со значением, возвращаемым функцией millis()
в основной программе, позволяет определить, что робот простаивает или вообще не подключен к системе управления, и принять решение об обесточивании
моторов: motor_off().
Листинг 4.2.3. Файл move_case.h
unsigned long flagTimeOff;
//==============================================
void move_case(char &bt_input)
{
if (bt_input != 'S')
{
flagTimeOff = millis() + 2500;
Собираем базовую модель
motor_on();
}
switch (bt_input) {
// Вперед
case 'F':
//
speed_left = maxSPEED;
speed_right = maxSPEED;
break;
// Назад
case 'B':
speed_left = -maxSPEED;
speed_right = -maxSPEED;
break;
// Влево
case 'L':
speed_left = -maxSPEED;
speed_right = maxSPEED;
break;
// Вправо
case 'R':
speed_left
= maxSPEED;
speed_right = -maxSPEED;
break;
// Прямо и влево
case 'G':
speed_left
= 0;
speed_right = maxSPEED;
break;
// Прямо и вправо
case 'I':
speed_left
= maxSPEED;
speed_right
= 0;
break;
// Назад и влево
case 'H':
speed_left
speed_right
= -maxSPEED;
= 0;
107
Глава 4
108
break;
// Назад и вправо
case 'J':
speed_right = -maxSPEED;
speed_left = 0;
break;
// Стоп
case 'S':
speed_left = 0;
speed_right = 0;
break;
// Скорость 0%
case '0':
maxSPEED = 300000;
break;
// Скорость 10%
case '1':
maxSPEED = 350000;
break;
// Скорость 20%
case '2':
maxSPEED = 400000;
break;
// Скорость 30%
case '3':
maxSPEED = 450000;
break;
// Скорость 40%
case '4':
maxSPEED = 500000;
break;
// Скорость 50%
case '5':
maxSPEED = 550000;
break;
// Скорость 60%
case '6':
maxSPEED = 600000;
break;
Собираем базовую модель
109
// Скорость 70%
case '7':
maxSPEED = 650000;
break;
case '8':
// Скорость 80%
maxSPEED = 700000;
break;
// Скорость 90%
case '9':
maxSPEED = 750000;
break;
// Скорость 100%
case 'q':
maxSPEED = 1000000;
break;
case 'V':
break;
case 'v':
break;
case 'X':
break;
case 'x':
break;
case 'D':
break;
}
// если требуемая скорость не равна с текущей.
if (speed_last_L != speed_left)
{
LEFT_STEPS_counter
= 2;
}
// если требуемая скорость не равна с текущей.
if (speed_last_R != speed_right)
{
RIGHT_STEPS_counter
}
}
= 2;
Глава 4
110
Выбор системы управления и основная программа
для робота
Последовательность этапов управления роботом при получении внешних команд показана на рис. 4.25. Первым делом должна быть проведена настройка
работы моторов — это отражено в блоке Определение моторов в программе.
Фактически к основной программе подключается модуль motorstep.h (#include
"motorstep.h"), а затем в функции setup() основной программы прописывается запуск функции: setup_motor_system().
Далее идет блок Настройка генерации шагов: к основной программе подключается модуль irq_robot.h (#include "irq_robot.h"), а затем в функции setup()
основной программы прописывается запуск функции: timers_setup().
Определим теперь, как робот будет получать команды управления, опишем это
в программе и запустим робота на тестирование.
В книге «Мобильные роботы на базе Arduino» рассматривались только две возможности управления: при помощи инфракрасного приемника от ИК-пульта
(как в бытовой технике) и по каналу Bluetooth. Также интересным и очень
эффективным является решение с использованием передатчика и приемника
для радиоуправляемых моделей (подключение подобного модуля рассмотрено по ссылке: http://zizibot.ru/
Начало
articles/electronics/flyskyi6/). Есть
и масса других решений — вплоть до
управления голосом, но выбор наш
Определение моторов в программе
будет основан на использовании имеющихся каналов связи с наибольшей
эффективностью.
Настройка генерации шагов
Получение
команды
Исполнение команды
моторами
Рис. 4.25. Последовательность этапов
управления роботом при получении внешних
команд
В состав контроллера ESP32 уже
включена поддержка беспроводных
каналов связи Wi-Fi и Bluetooth. Но
для управления по Wi-Fi нужно организовать в помещении сеть, а для
управления по Bluetooth потребуется
только смартфон, поэтому выберем
Bluetooth. Поскольку большинство
смартфонов работают на ОС Android,
выберем соответствующую программу — Arduino Bluetooth RC Car. Она
управляет движением робота, посылая
Собираем базовую модель
111
ему с интервалом 50 миллисекунд последовательности команд-символов. Коды
команд соответствуют приведенным в табл. 4.1, остается их получить и правильно обработать. Обработка команд была рассмотрена в листинге 4.2.3, с подключением по Bluetooth мы познакомились в разд. «Тестирование поддержки
Bluetooth» главы 2.
В каталоге основной программы (см. папку listing_4_2 сопровождающего книгу файлового архива), которую мы сейчас создаем, должны уже находиться ранее рассмотренные программные файлы: motorstep.h, irq_robot.h и move_case.h.
Основная программа, соответственно, будет называться listing_4_2.ino и располагаться в той же папке. На рис. 4.26 показано, как будет выглядеть наш проект
после открытия в Arduino IDE — здесь присутствуют четыре вкладки с именами
упомянутых программных модулей.
Итак, рассмотрим файл listing_4_2.ino (его содержимое приведено в листинге 4.2.4):
• директивы: #include "irq_robot.h", #include "irq_robot.h" и #include
"move_case.h" — включают тексты этих файлов в указанные места программного листинга при компиляции программы;
• функция: setup() — инициализирует управление моторами и открывает
порт общения по интерфейсу Bluetooth. В основном цикле функции loop()
вызывается только одна функция: move_case(), которая проверяет наличие новых команд и управляет моторами;
• условие: if(SerialBT.available()) — проверяет наличие команд от
смартфона в буфере контроллера. Если команды есть, они обрабатываются
в операторе выбора switch (bt_input), и текущие скорости колес корректируются на величину, не превышающую dSpeed, в сторону, которая соответствует принятой команде;
• строка: if (flagTimeOff < millis()) motor_off() — проверяет, когда
последний раз приходила команда на продолжение движения (кроме остановки). Если это время превышает 2500 миллисекунд, моторы робота обесточиваются. Это позволяет существенно экономить заряд батарей робота.
Листинг 4.2.4. Файл listing_4_2.ino
// Подключаем библиотеку для работы с Bluetooth
#include <BluetoothSerial.h>
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and
enable it
#endif
Глава 4
112
Рис. 4.26. Программа управления роботом в Arduino IDE
Собираем базовую модель
113
// Создаем Bluetooth-порт
BluetoothSerial SerialBT;
// Подключаем внутренние программные файлы.
#include "motorstep.h"
// Описание моторов
#include "irq_robot.h"
// Работа с прерываниями
#include "move_case.h"
// Реагирование на команды
//=====================================
//+++++++++++++++++++++++++++++++++++++
//=====================================
void setup() {
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы
motor_off();
// Инициализируем генерацию шагов моторов
timer_setup();
// Открываем Serial-порт
Serial.begin(115200);
// Открываем Bluetooth-порт
SerialBT.begin("Hacker1"); // Bluetooth имя робота.
Serial.println("Do");
}
//++++++++++++++++++++++++++++++++++++++
void loop()
{
// Проверяем наличие команд от смартфона
if (SerialBT.available())
{
// Читаем из буфера очередной символ
char bt_input = (char)SerialBT.read();
// Обрабатываем принятую команду
move_case(bt_input);
}
else // Если нет связи со смартфоном, отключить драйверы
if (flagTimeOff < millis()) motor_off();
}
Итак, программа создана. Произведем ее загрузку в контроллер робота. И перейдем к разделу «Пробный старт».
Глава 4
114
Пробный старт
Робот собран, программа загружена в ESP32. Скачиваем на смартфон из Google
Play программу Arduino Bluetooth RC Car. Включаем робота, в разделе настроек Bluetooth смартфона включаем синхронизацию с устройством Hacker1 (так
мы назвали робота в программе). Переходим в программу RC Car и выбираем управление нашим роботом из меню подключенных Bluetooth-устройств
(рис. 4.27). Если робот подключился, то в программе высветится зеленый значок (рис. 4.28). Нам осталось только управлять движением робота стрелками
программы RC Car с экрана смартфона.
Рис. 4.27. Выбор Bluetooth-устройства
Рис. 4.28. Соединение робота со смартфоном по Bluetooth установлено
Собираем базовую модель
115
Задание
• Управляя бегунком, расположенным в правом верхнем углу программы RC
Car (см. рис. 4.28), измените максимальную скорость робота. Как реагирует
робот на увеличение/уменьшение максимальной скорости?
• Измените имя робота в программе контроллера. Задайте роботу новое имя
в Bluetooth-окружении. Перерегистрируйте робота на смартфоне.
Неполадки и пути их устранения
Пути устранения возможных неполадок приведены в табл. 4.2.
Таблица 4.2. Порядок устранения неисправностей
Проявление Неисправность
Порядок устранения
Колеса вращаются не в
ту сторону
Решение 1:
Направление
вращения колеса задано неверно
в файле motor_step.h для нужного колеса инвертировать
значения констант:
#define
#define
#define
#define
DIR_L_FORWARD false
DIR_L_BACKWARD true
DIR_R_FORWARD true
DIR_R_BACKWARD false
Они отвечают за направление вращения колеса
Решение 2:
Перекинуть полярность одной фазы на шаговом моторе
Вперед и
назад робот
едет правильно, а поворачивает
неправильно
Перепутаны
контакты
управления для
правого и левого колеса
Решение 1:
в файле motor_step.h поменять местами номера GPIO,
отвечающие за колеса:
#define STEP_L 15 // Пин левого шага
#define STEP_R 4 // Пин правого шага
#define STEPER_EN 2 // Пин включения моторов
// Функция инициализации управления моторами
#define DIR_L 13 // Пин направления левого шага
#define DIR_R 16 // Пин направления правого шага
Решение 2:
Поменять провода на роботе согласно схеме
Колеса пищат, но не
стартуют
Высокая стартовая скорость
и/или ускорение
в файле motor_step.h отрегулируйте указанные величины
int32_t maxSPEED = 800000;
int32_t minSPEED = 100000;
int32_t
acceleration = 20;
116
Глава 4
Выводы
Для дальнейшего понимания всего материала книги эта глава является базовой.
Если у вас остались вопросы по работе робота, рекомендуем обратиться к дополнительным материалам и добиться полного понимания. Все последующие
проекты опираются на базовую модель робота и базовую программу управления, так или иначе модифицируя и дополняя их.
В следующей главе мы научим робота запоминать свой путь и повторять его по
памяти. Для этого в конструкцию робота придется внести небольшие дополнения: пару кнопок и адресный светодиод — все остальное уже сделано.
ГЛАВА 5
УЧИМ РОБОТА САМОСТОЯТЕЛЬНО
ПОВТОРЯТЬ ПРОЙДЕННЫЙ ПУТЬ
Представим ситуацию: мы управляем роботом посредством дистанционного
пульта, он от места старта проходит по нашим командам определенный путь
до места финиша — пусть это будет прохождение территории с препятствиями. Затем устанавливаем робота в место старта и ставим ему задачу повторить
пройденный путь самостоятельно. Если робот повторяет путь, цель считается
достигнутой.
Робот должен работать в двух режимах:
• режим записи, когда мы им управляем удаленно, а он запоминает информацию, достаточную для повторения пути;
• режим повторения, когда робот по памяти проходит ранее пройденный
путь.
Благодаря использованию шаговых моторов в качестве приводов колес, мы
имеем возможность точно (по шагам) позиционировать робота. Получая также
информацию о времени, за которое роботом сделан тот или иной шаг, и записав
ее, мы сможем в дальнейшем «прокрутить» запись, повторяя шаги.
Итак, наша цель: научить робота запоминать пройденный путь и повторять его
даже после перезагрузки (выключения питания).
Шаги моторов робота всегда имеют строго определенную длину — то есть вал
за один шаг (команду шага) поворачивается на определенный угол (см. главу 3).
Этот угол настраивается в драйвере мотора и в дальнейшем не меняется.
Изменяя время между шагами, мы управляем скоростью вращения моторов, которая обратно пропорциональна времени между шагами.
Текущую скорость вращения моторов и количество шагов, сделанных на этой
скорости, можно извлечь из программы (см. главу 4), записать в память и в
Глава 5
118
дальнейшем использовать. Поскольку колеса робота одинаковы, нам не важен
их диаметр, база между колесами или реальная скорость перемещения робота, — достаточно информации в относительных единицах.
На рис. 5.1 представлена наглядная демонстрация изменения скоростей моторов робота при движении (процессы разгона и торможения моторов для простоты исключены). Робот стартует из точки 1, и до момента начала движения
начальные скорости обоих моторов равны нулю (шаги не генерируются). Затем
из точки 1 в точку 2 робот движется по прямой — скорости обоих его моторов
равны как по знаку, так и по значению (время между шагами обоих моторов
и количество сделанных шагов для обоих моторов совпадают). Из точки 2 в точку 3 робот движется по дуге: для движения по дуге правый двигатель уменьшает
скорость до величины 0.5 × Vmax (при этом в программе для него сигналы шагов
генерируются реже). Из точки 3 в точку 4 робот снова движется прямо. В точке 4
робот поворачивает на месте: для этого левый двигатель заданное время делает
шаги вперед, а правый — меняет направление вращения на обратное (при этом
количество шагов, сделанных обоими моторами, совпадает, а угол реального
поворота робота зависит от количества шагов и диаметра колеса). Из точки 4
в точку 5 робот опять движется прямолинейно и, сделав определенное количество шагов, замирает в точке 5.
3
VL = VR = Vmax
4 VL = Vmax
VR = –Vmax
VR = 0,5 × Vmax
VL = VR = Vmax
2
VL = VR = Vmax
VL = Vmax
5 ФИНИШ
VL = 0
VR = 0
1
VL = 0
VR = 0
СТАРТ
Рис. 5.1. Демонстрация маршрута робота
Учим робота самостоятельно повторять пройденный путь
119
Конечно, поверхность, по которой движется робот, не идеальна, и в реальности
колеса могут и пробуксовывать, поэтому повторенный путь будет немного отличаться от пройденного.
Если бы ставилась задача провести робота по маршруту, составленному на основе замеров длины отрезков пути и углов поворота (например, линейкой и транспортиром), то нам дополнительно потребовалось бы включить в расчет диаметр
колес и величину базы между колесами. Однако при повторе уже пройденного
маршрута такая информация не требуется.
Для функционирования робота в упомянутых ранее режимах (режим записи
и режим повторения) нам потребуется решить задачи хранения информации о
маршруте, чтения ее из памяти и выбора режима работы робота.
Обоснование системы хранения
записанного маршрута
Контроллер ESP32 — это практически микрокомпьютер, он несет на борту
520 килобайт оперативной памяти, часть которой уже занята выполняемой
программой и переменными, а незанятую часть можно использовать. Объем
свободной оперативной памяти легко узнать при помощи команды: ESP.
getFreeHeap() и вывести в Монитор порта по команде: Serial.println(ESP.
getFreeHeap()).
Программа управления роботом из главы 4 оставила незанятым объем свободной оперативной памяти порядка 170 килобайт. Этого вполне достаточно, чтобы объявить несколько небольших массивов и записать в них информацию о
маршруте для проведения экспериментов. Но если робота выключить, вся информация, находящаяся в оперативной памяти контроллера, будет «забыта»...
Поэтому, если нужно сохранить данные и использовать их после повторного
включения робота, следует задействовать энергонезависимую память ESP32,
часть которой доступна для сохранения пользовательских данных.
В модуле контроллера ESP32, как правило, имеется 4 мегабайта энергонезависимой памяти, из которой нам будет в качестве небольшой Flash-карты доступно порядка 20 килобайт, — остальная память забирается под другие нужды либо
зарезервирована (например, в этой памяти хранится код программы, который
считывается в оперативную память в момент включения контроллера). Заметим,
что 20 килобайт — это совсем не много, и маршрут нам удастся записать весьма
короткий. Этого может быть достаточно для проверки некоторых интересных
идей, а для более объемных записей следует применять носитель SD-card.
120
Глава 5
Работа с энергонезависимой памятью контроллера
ESP32
В комплекте примеров Arduino IDE есть подходящий пример, который можно
открыть по команде меню Примеры | Preferences | StartCounter. На его основе сделана простенькая программа (листинг 5.1), запоминающая количество
миллисекунд от начала работы программы.
Программа записывает в хранилище storage энергонезависимой памяти ключ
sDS с количеством текущих миллисекунд от начала работы программы. Если
в открытом Мониторе порта (на скорости 115 200) ввести цифру и нажать клавишу <Enter>, значение ключа будет занесено в энергонезависимую память,
а если ввести символ a (нажать клавишу <a>), значение ключа будет считано
из хранилища, выведено в порт, и мы его увидим в Мониторе порта.
Для удобной работы с энергонезависимой памятью разработана библиотека
Preferences.h, и ее нужно предварительно подключить в программе:
#include <Preferences.h>
Далее создается объект типа Preferences, в программе он имеет имя
preferences, но имя может быть изменено:
Preferences preferences
Для создания нового или использования старого хранилища данных используется команда: preferences.begin("имя хранилища"), в примере это:
preferences.begin("storage")
Для записи в хранилище определенного ключа служит команда:
preferences.putULong("sDS", serialDataStorage1)
Здесь: sDS — имя ключа, serialDataStorage1 — переменная, содержащая 4 байта данных. У нас переменной serialDataStorage1 предварительно присваивается значение пройденного времени работы программы в миллисекундах:
serialDataStorage1 = millis()
Листинг 5.1. Пример работы с внутренней энергонезависимой памятью
// Подключаем библиотеку.
#include <Preferences.h>
// Создаем объект хранилище.
Preferences preferences;
Учим робота самостоятельно повторять пройденный путь
void setup() {
// Открытие Serial-порта
// на скорости 115 200 бит/сек (Бод).
Serial.begin(115200);
Serial.println("Open Storage");
// Открываем хранилище с именем storage.
// Второй параметр false - режим записи/чтения.
// Длина имени не более 15 симв.
Serial.println(preferences.begin("storage"));
// Удаление всех хранилищ
//preferences.clear();
// Удаление отдельного ключа
//preferences.remove("counter");
}
// Бесконечный цикл loop.
void loop() {
// Если в порту есть данные
if (Serial.available())
{
// Читаем один байт из порта.
byte Sdata = Serial.read();
// Если код введенной буквы соответствует
// значению кода цифр от 0 до 9 (код 48-57)
if ((Sdata > 47) && (Sdata < 58))
{
// Сохранить значение времени от начала работы программы
// под именем "serialDataStorage"
Serial.print("Writing...");
unsigned long serialDataStorage1 = millis();
Serial.println(serialDataStorage1);
preferences.putULong("sDS", serialDataStorage1);
}
else
{
if (Sdata == 'a')
{
121
Глава 5
122
// Если введена буква 'a'
// Прочитать значение ключа с указанным именем,
// если ключа нет, вернуть 0
unsigned long serialDataStorage2 =
preferences.getULong("sDS");
Serial.print("Read...");Serial.println(serialDataStorage2);
}
}
}
}
Помимо функции для записи целых четырехбайтовых значений:
preferences.putULong("sDS", serialDataStorage1)
в библиотеке Preferences.h описан целый ряд функций для записи чисел
другого формата, а также для записи массивов, структур и строк. Далее приведен список полезных функций записи/чтения для энергонезависимой памяти.
Работа с ними проста и понятна из синтаксиса:
• key — строковое имя ключа;
• value — записываемая информация;
• len — длина записываемых данных (для массивов).
Каждая из далее приведенных функций возвращает значение — это количество
записанных байтов. Если запись не удалась, будет возвращен 0.
Функции записи
size_t putChar(const char* key, int8_t value);
size_t putUChar(const char* key, uint8_t value);
size_t putShort(const char* key, int16_t value);
size_t putUShort(const char* key, uint16_t value);
size_t putInt(const char* key, int32_t value);
size_t putUInt(const char* key, uint32_t value);
size_t putLong(const char* key, int32_t value);
size_t putULong(const char* key, uint32_t value);
size_t putLong64(const char* key, int64_t value);
size_t putULong64(const char* key, uint64_t value);
size_t putFloat(const char* key, float_t value);
Учим робота самостоятельно повторять пройденный путь
123
size_t putDouble(const char* key, double_t value);
size_t putBool(const char* key, bool value);
size_t putString(const char* key, const char* value);
size_t putString(const char* key, String value);
size_t putBytes(const char* key, const void* value, size_t len);
Функции чтения
int8_t getChar(const char* key, int8_t defaultValue = 0);
uint8_t getUChar(const char* key, uint8_t defaultValue = 0);
int16_t getShort(const char* key, int16_t defaultValue = 0);
uint16_t getUShort(const char* key, uint16_t defaultValue = 0);
int32_t getInt(const char* key, int32_t defaultValue = 0);
uint32_t getUInt(const char* key, uint32_t defaultValue = 0);
int32_t getLong(const char* key, int32_t defaultValue = 0);
uint32_t getULong(const char* key, uint32_t defaultValue = 0);
int64_t getLong64(const char* key, int64_t defaultValue = 0);
uint64_t getULong64(const char* key, uint64_t defaultValue = 0);
float_t getFloat(const char* key, float_t defaultValue = NAN);
double_t getDouble(const char* key, double_t defaultValue = NAN);
bool getBool(const char* key, bool defaultValue = false);
size_t getString(const char* key, char* value, size_t maxLen);
String getString(const char* key, String defaultValue = String());
size_t getBytes(const char* key, void * buf, size_t maxLen);
Функции очистки хранилища
Энергонезависимая память очень быстро заполняется записываемыми в нее
данными, поэтому лишние записи из нее нужно удалять. Для этого служат следующие функции:
• bool clear() — очистка текущего хранилища (из текущего хранилища
удаляются все ключи);
• bool remove(const char * key) — удаления ключа с указанным именем
из хранилища.
Эти методы работают только с хранилищем, открытым функцией: begin("имя
хранилища"). И если вы не укажете имена созданных и заполненных хранилищ,
то свободное место в энергонезависимой памяти вашего контроллера может закончиться, и запись новых данных производиться не будет. Стандартными методами библиотеки Preferences.h эту проблему решить нельзя.
124
Глава 5
Для полной очистки энергонезависимой памяти надо подключить библиотеку
nvs_flash.h:
#include <nvs_flash.h>
а в теле программы вызвать функцию nvs_flash_erase(). После выполнения
этой операции все пользовательские хранилища в энергонезависимой памяти
ESP32 будут стерты, и место под запись новой информации освободится.
Обоснование выбора элементов
управления
Робот должен управляться с пульта или работать в автономном режиме. Поэтому
следует предусмотреть в проекте переключатель режимов работы робота и индикатор режимов его работы.
Выбор способа переключения режимов
Для переключения между режимами работы можно применить:
• программу для смартфона;
• веб-интерфейс робота;
• кнопки на самом роботе.
Однако программа для смартфона будет задействована нами при записи маршрута, и создавать дополнительную пока нецелесообразно.
Организация веб-интерфейса несколько усложняет простую задачу, поэтому его
мы применим в одной из следующих глав — там, где он будет действительно необходим.
А вот использование управляющих кнопок — хорошее и несложное решение,
которое к тому же расширит наши знания по схемотехнике. Технически самым
простым является решение с подключением на определенный контакт GPIO
кнопки, нажатие на которую переключает состояние GPIO с высокого на низкий
уровень (рис. 5.2).
Если кнопка не нажата, напряжение питания через резистор R3 подается на контакт GPIO 32, и на этом контакте будет высокий уровень сигнала. Если нажать
кнопку, то она окажется замкнутой на «землю», при этом небольшой ток через резистор R3 станет стекать к GND, и на GPIO 32 получится низкий уровень
сигнала. Чтение текущего уровня сигнала в программе можно осуществлять
Учим робота самостоятельно повторять пройденный путь
125
командой: digitalRead("номер GPIO"), а записывать в переменную для дальнейшего анализа состояния GPIO 32 командой: Button = digitalRead(32),
где Button — имя переменной, в которой хранится 0, если кнопка нажата, или 1,
если кнопку не нажимать.
1
R3
10 кОм
2
Длительностью нажатия на кнопку может выбираться режим работы: короткое нажатие — запись маршрута, длительное — воспроизведение. Схема подключения кнопки в этом случае также
3,3 V
достаточно проста, но усложняется
программа обработки, увеличиваются
задержки на выбор режима, появляется существенная вероятность неправильного срабатывания из-за дребезга контактов кнопки.
1
Возможно также аналогичное подключение нескольких кнопок — по
количеству режимов, но это приведет
к нерациональному использованию
контактов GPIO, которые могут потребоваться в дальнейшем.
Следующий вариант заключается в
подключении нескольких кнопок на
один контакт GPIO, если он «умеет»
считывать величину аналогового сигнала. В этом случае следует обеспечить
условие, когда нажатие определенной
кнопки приводит к значительно отличающемуся от нажатия другой кнопки
изменению величины напряжения на
задействованном контакте GPIO.
Схема этого варианта — с двумя кнопками на одном аналоговом контакте
GPIO — приведена на рис. 5.3. Правые
контакты кнопок S1 и S2 объединены, подключены к контакту GPIO 32
(аналоговый вход) и через резистор R3
(10 кОм) к питанию 3,3 вольта, а левые
контакты через резисторы различной
величины: R1 (10 кОм) и R2 (1 кОм) —
подключены к GND («земле»).
1
S1
2
2
U1
3V
GND
EN
TX
VP
RX
VN
3V
32
22
33
21
LOLIN32
V1.0.0 GND
34
35
GND
25
19
26
23
27
18
14
5
12
3V
13
17
5V
16
GND
4
0
+Bat
GND
GND
2
15
3,3 V
Рис. 5.2. Электрическая схема подключения
управления с одной кнопкой
Глава 5
126
При нажатии на кнопку S1 ток начинает течь через резисторы R3 и R1, и напряжение на GPIO 32 будет равно падению напряжения на R1:
IR1 = Uпит/(R3+R1),
Ugpio32 = IR1 × R1 = R1 × Uпит /(R3+R1),
Ugpio32 = 10000 Ом × 3,3 В/(10000 Ом + 10000 Ом) = 1,65 В,
Ugpio32 = 1,65 В — это половина от напряжения питания.
При нажатии на кнопку S2 ток начинает течь через резисторы R3 и R2, и напряжение на GPIO 32 будет равно падению напряжения на R2:
IR1 = Uпит/(R3+R2),
Ugpio32 = IR1 × R2 = R2 × Uпит /(R3+R2),
Ugpio32 = 1000 Ом × 3,3 В/(10000 Ом + 1000 Ом) = 0,3 В.
Ugpio32 = 0,3 В — это напряжение существенно отличается
как от напряжения питания, так и от напряжения, которое появляется
при нажатии кнопки S1.
В программе аналоговые значения считываются командой analogRead("номер
GPIO") — по умолчанию диапазон значений изменяется линейно от 4000 для
напряжения питания до 0 при полном отсутствии напряжения (когда контакт
GPIO подключен к контакту GND).
В нашем случае при нажатии кнопки S1 считывается значение, близкое к:
(4000 × 1,65 В)/ 3,3 В = 2000.
При нажатии кнопки S2 считывается значение, близкое к:
(4000 × 0,3 В)/ 3,3 В = 363.
Если случайно нажать на обе кнопки:
Ugpio32 = ( (R2 × R1) / (R2 + R1) ) × Uпит / ( R3+ (R2 × R1) / (R2 + R1) ),
Ugpio32 = 0,275 В.
Значение, считанное в программе, в этом случае будет равно:
(4000 × 0,275 В)/ 3,3 В = 333.
Следует заметить, что реально считанные значения будут колебаться в пределах
плюс-минус 30 единиц — это связано как с особенностью аналого-цифрового
преобразования в ESP32, так и с бросками питающего напряжения. Эту
Учим робота самостоятельно повторять пройденный путь
127
особенность нужно при анализе в программе иметь в виду. Могут немного отличаться от номинала и сами резисторы, поэтому рекомендуется на практике
измерить полученные значения и их разброс и учесть все это при программировании.
Надо также учесть и то, что аналоговое чтение в ESP32 осуществляется приблизительно за 10 микросекунд. Значит, на анализ состояния нажатия кнопок эти
10 микросекунд и потребуются.
1
R3
10 кОм
2
3,3 V
R1
10 кОм
1
2
2
S1
1
1
R2
1 кОм
1
1
1
2
2
S2
2
2
U1
3V
GND
EN
TX
VP
RX
VN
3V
32
22
33
21
LOLIN32
V1.0.0 GND
34
35
GND
25
19
26
23
27
18
14
5
12
3V
13
17
5V
16
GND
4
0
+Bat
GND
GND
2
15
3,3 V
Рис. 5.3. Электрическая схема подключения управления с двумя кнопками на одном
аналоговом контакте GPIO
128
Глава 5
Выбор индикатора режима
В качестве индикатора режима работы (состояния робота) можно использовать
следующие элементы:
• зуммер — сигнализирует звуком определенной частоты и длительности;
• светодиод — сигнализирует светом и миганием с заданной частотой;
• дисплей — выводит подробную информацию о состоянии робота;
• программу для смартфона с возможностью получения информации от робота и отображения на дисплее смартфона;
• веб-интерфейс робота;
• адресный (многоцветный) светодиод — сигнализирует яркостью, цветом и
миганием с заданной частотой.
Зуммер и светодиод имеют схожее подключение на один GPIO. Это неплохое и
простое решение, но если состояний робота более двух, требуется время, чтобы
распознать соответствующие звуки или световые сигналы.
Использование дисплея существенно расширяет возможности робота по выводу информации, но для нашего «простого» робота этого пока не требуется.
Программа для смартфона — также хорошее решение, но для создания такой
программы требуются определенные усилия и знания, что выходит за рамки
этой книги.
К веб-интерфейсу мы уже обращались в главе 2, когда следили за температурой
ядер контроллера. Обязательным условием его применения является подключение робота к сети Wi-Fi. Мы обратимся к этому способу позднее.
А вот адресный (многоцветный) светодиод прост в подключении, информацию
его видно на расстоянии до 10 метров, запомнить различные цвета по состояниям довольно легко, и не требуется выполнения дополнительных условий — например, наличия Wi-Fi.
Поэтому мы выберем для нашего проекта адресный многоцветный светодиод
WS2812B. У него имеется информационный вход, подавая на который различные сигналы, можно управлять яркостью и спектром свечения. Мы закрепим
разные цвета этого светодиода за различными режимами-состояниями робота,
но сначала рассмотрим, как работает такой светодиод.
Что такое адресный светодиод?
Адресный светодиод (рис. 5.4) — сложное устройство, внутри которого помимо
разноцветных светодиодов имеется схема управления.
Учим робота самостоятельно повторять пройденный путь
129
Адресные светодиоды можно соединять последовательно и управлять ими всеми, используя один информационный проводник GPIO, при этом все светодиоды управляются отдельно и могут светиться по-разному. Из подобных светодиодов делают, например, цветомузыку и информационные панели. Монтажная
и электрическая принципиальная схемы подключения адресных светодиодов
приведены на рис. 5.5 и 5.6 соответственно.
+ питания
Вход
управления
Выход
управления
– питания
Рис. 5.4. Адресный светодиод WS2812B
Рис. 5.5. Монтажная схема подключения адресных светодиодов
Глава 5
130
Адресный
U1
5V
3V
GND
EN
TX
VP
RX
VN
3V
32
22
33
21
LOLIN32
V1.0.0 GND
34
35
GND
25
19
26
23
27
18
14
5
12
3V
13
17
5V
16
GND
4
0
+Bat
GND
GND
2
15
3 светодиод 2
VSS
2
1
DOUT
VDD
4
WS2812b
DIN
5V
RGB LED
2
4
3
VSS
DOUT
DIN
Адресный
светодиод 1
VDD
5V
1
WS2812b
RGB LED
Адресный
светодиод
3
VSS
2
1
DOUT
VDD
4
WS2812b
DIN
5V
RGB LED
Рис. 5.6. Электрическая принципиальная схема подключения адресных светодиодов
Работает все это так: схема управления первого светодиода получает управляющий пакет, первую управляющую команду забирает и выполняет, а остальные
передает далее, следующий светодиод по цепочке поступает абсолютно так же,
пока последовательность управляющих команд в пакете не закончится или не
закончатся светодиоды в цепочке.
Пакет сигналов управления светодиодами формируется с использованием
библиотеки Adafruit_NeoPixel.h. Установите ее из Менеджера библиотек
(рис. 5.7), а затем подключите в своей программе командой:
#include <Adafruit_NeoPixel.h>.
Для того чтобы работать со светодиодами, нужно создать объект типа:
Adafruit_NeoPixel, Adafruit_NeoPixel pixels = Adafruit_NeoPixel(N, GPIO, REG)
Учим робота самостоятельно повторять пройденный путь
131
Рис. 5.7. Библиотека Adafruit NeoPixel в Менеджере библиотек
Здесь: первый параметр — количество светодиодов, второй — контакт GPIO
управления, третий — режим работы.
Вызов функции: pixels.begin() — разрешает управление светодиодами.
Вызов функции: pixels.setPixelColor(0, pixels.Color(255, 255, 255)) —
создает команду управления определенным светодиодом: первый параметр —
номер светодиода, второй — яркость по трем составляющим спектра.
Вызов команды: pixels.show() — отправляет сформированный пакет на светодиоды. Если перед этим формировались команды для различных светодиодов, все они будут применены только после выполнения этой команды.
Установка элементов управления на робота
Кнопки выбора режима
Для управления роботом и выбора режима его работы подключим пару кнопок:
S1 и S2 — на один общий контакт GPIO 32. Монтажная и электрическая принципиальная схемы подключения кнопок приведены на рис. 5.8 и 5.9 соответственно. Здесь также показано подключение адресного светодиода к управляющему
им контакту GPIO 5 (о подключении этого светодиода к питанию см. далее, на
рис. 5.10 и 5.11).
Простая программа проверки работы кнопок приведена в листинге 5.1, a. Здесь
команда Serial.println(analogRead(32)) выводит в Монитор порта аналоговые значения с контакта GPIO 32. Запомним, какие значения получаются
при нажатии каждой из кнопок — эти значения станут базовыми в программе
управления при выборе режима работы робота. В модуле setup() не забудьте
открыть порт, через который будут поступать данные от контроллера: Serial.
begin(115200).
132
Глава 5
Рис. 5.8. Монтажная схема подключения кнопок и управления адресным светодиодом
Листинг 5.1, а. Проверка работы кнопок
void setup()
{
Serial.begin(115200);
}
void loop()
{
Serial.println(analogRead(32));
delay(200);
}
Учим робота самостоятельно повторять пройденный путь
Вкл. режим прохода по записи
1
R1
10 кОм
1
2
S1
1
2
1
R3
10 кОм
133
3,3 V
2
2
Адресный светодиод
Вкл. режим записи
1
R2
1 кОм
1
S2
2
2
4
2
1
2
3
VSS
DOUT
DIN
VDD
1
WS2812b
RGB LED
U1
3V
GND
EN
TX
VP
RX
VN
3V
32
22
33
21
LOLIN32
V1.0.0 GND
34
35
GND
25
19
26
23
27
18
14
5
12
3V
13
17
5V
16
GND
4
0
+Bat
GND
GND
2
15
3,3 V
Рис. 5.9. Электрическая принципиальная схема подключения кнопок и управления адресным
светодиодом
Глава 5
134
Информационный адресный светодиод
В разд. «Что такое адресный светодиод?» показан общий случай подключения
нескольких (трех) адресных светодиодов, в нашем же случае достаточно иметь
один светодиод, который светится или мигает определенным светом в зависимости от того, в каком состоянии находится робот. Монтажная и электрическая принципиальная схемы подключения светодиода к питанию приведены на
рис. 5.10 и 5.11 соответственно.
Напомним, что для управления светодиодом мы выделили контакт GPIO 5
(см. также рис. 5.8 и 5.9).
Рис. 5.10. Монтажная схема подключения адресного светодиода
Учим робота самостоятельно повторять пройденный путь
135
Адресный светодиод
2
4
WS2812b
RGB LED
3
VSS
DOUT
DIN
VDD
U1
1
14,8–16V
S2
1
2
2
1
2
2
C2
0,1mF
16V
3
1
2
OUT
+
GND
78XX
1
C3
0,1mF
10V
IC2
IN
1
+
2
GND
3V
TX
EN
RX
VP
VN
3V
22
32
21 LOLIN32
33
34
GND V1.0.0
GND
35
19
25
23
26
18
27
5
14
3V
12
17
13
16
5V
4
GND
0
GND
+Bat
2
GND
15
Рис. 5.11. Электрическая принципиальная схема подключения адресного светодиода
Программирование
Описание алгоритма работы робота
Записываем маршрут
Видов записи маршрута весьма много, как и протоколов воспроизведения.
Отдавая себе отчет в том, что алгоритм генерации шага, описанный в главе 4,
уже достаточно сложен, мы здесь предлагаем простое и эффективное решение,
позволяющее записать маршрут в таком виде, который было бы не сложно понять. Запись эта также проста и для повторения.
136
Глава 5
Благодаря тому, что каждые 10 микросекунд в программе вызывается функция
обработки прерывания, можно точно определить момент, когда от системы
управления пришла команда на изменение скорости (команда управления роботом), и запомнить само значение новой скорости (скорости вращения моторов), а также время по счетчику прерываний от начала работы режима записи.
Таким образом, для каждого мотора достаточно пары массивов:
• массив, содержащий время, когда пришла команда на изменение скорости;
• массив, содержащий новую скорость вращения мотора.
Время изменения скорости, когда пришла команда на изменение скорости,
в десятках микросекунд от начала работы, — это точный параметр, и он будет
храниться в 4-байтовой ячейке массива. А вот скорость каждого мотора в результате работы функции move_case() (она обрабатывает входящие команды
и рассматривалась в главе 4) принимает только три значения: минус MAXspeed,
ноль, плюс MAXspeed. Значит, при умелом обращении можно для ее хранения
использовать только 1 байт — поскольку экономия места при записи в энергонезависимую память чрезвычайно важна.
Соответственно, для двух моторов нам понадобятся 4 массива:
• int8_t array_speed_left[max_i];
• uint32_t array_timer_counter_left[max_i];
• int8_t array_speed_right[max_i];
• uint32_t array_timer_counter_right[max_i],
где max_i — размер массива.
Рассчитаем занятую память: 1000 × (4 × 2 + 1 × 2) = 10 000 байтов будет занято
в оперативной памяти для хранения протокола записи пути для 1000 значений
изменения скорости. Для записи в энергонезависимую память (напомним, ее
свободный объем составляет около 20 Кбайт) разумно этой величиной и ограничиться. Если же записывать этот протокол в оперативную память (а ее объем
около 170 000 байтов), путь можно значительно увеличить, но после выключения робота протокол будет утерян.
Вернемся к рис. 5.1 и проанализируем, сколько ячеек массива будет потрачено
на маршрут. Считая, что каждый участок заканчивается остановкой и лишь затем начинается следующий, на каждый участок будет потрачено по два элемента
массива: первый задает скорость движения, а второй, в конце участка, — команда торможения до остановки. Итого будет записано по 10 ячеек и останется свободными еще по 990 для каждого мотора. Если же считать, что робот движется
без перерывов, то для левого мотора будет занято всего 2 элемента массива — так
Учим робота самостоятельно повторять пройденный путь
137
как его скорость не меняется до торможения в точке 5, а для правого мотора
будет занято 6 ячеек памяти — его скорость меняется в каждой точке плюс торможение в точке 5.
Воспроизводим записанный маршрут
При воспроизведении эмулируется поведение системы управления. Программа
ожидает момента времени, записанного в ячейке массива, хранящей время следующей команды, и изменяет параметры скорости согласно параметрам скорости, записанным в массиве. Затем счетчик элементов массива смещается на 1 и
снова ожидается совпадение текущего времени и времени в ячейке массива.
Это все просто. Гораздо объемнее выглядит выбор режимов работы и функции
сохранения данных в энергонезависимую память и чтения из нее, схемы алгоритмов которых мы сейчас и рассмотрим.
Выбираем режимы работы
Алгоритм выбора режима работы представлен на рис. 5.12 (начало) и 5.13
(окончание).
После включения режим работы еще не выбран, и программа должна постоянно проверять нажатия на кнопки. Если нажата кнопка записи, флаг режима
operating_mode принимает значение WRITE_MODE — этот флаг проверяется не
только в основной программе, но и в функции обработчике прерываний, которая запускается каждые 0 микросекунд.
В режиме записи в основной программе проверяется только состояние простоя
робота. Если робот простаивает, его моторы отключаются, а сведения о пройденном маршруте записываются в энергонезависимую память.
Если выбран режим воспроизведения, сначала производится попытка считать
данные из энергонезависимой памяти, и в случае успеха робот переводится
в состояние READ_MODE — повторение маршрута. Это состояние также анализируется в функции обработки прерываний, а в основной программе проверяется только выполнение роботом последней команды, после чего моторы
отключаются.
Работа функций сохранения данных в энергонезависимую память
и чтения из нее
Алгоритм обработки состояния WRITE_MODE в функции обработки прерывания
по таймеру step_timer() показан на рис. 5.14.
Глава 5
138
Начало
operating_mode = NO_MODE
1
Нет
Да
operating_mode == NO_MODE
(Если робот в режиме ожидания)
2
Button = analogRead(32)
(Опрос состояния GPIO 32)
Button == button_Do
(Если нажата кнопка
повторения маршрута)
Нет
Да
Button == button_Write
(Если нажата кнопка
записи маршрута)
read_from_flash()
(Произвести чтение из
энергонезависимой памяти данных
пройденного пути)
Нет
1
Да
operating_mode = WRITE_MODE
(Перейти в режим записи маршрута)
Нет
1
Если чтение
прошло удачно
1
Да
operating_mode = READ_MODE
(Перейти в режим чтения маршрута)
1
Рис. 5.12. Алгоритм выбора режима работы (начало)
Учим робота самостоятельно повторять пройденный путь
139
2
Да
operating_mode == WRITE_MODE
(Режим записи)
Нет
operating_mode == READ_MODE
(Режим чтения)
Нет
1
Если пришла
новая команда
Да
Да
bt_input = (char)SerialBT.read()
(Получение новой команды)
Нет
write_counter_Rmax <= write_counter_R
И
write_counter_Lmax <= write_counter_L
(Если выполнены все записанные
команды)
move_case(bt_input)
(Обработка команды)
1
Да
Отключить
моторы
Да
flagTimeOff < millis()
(Если тайм-аут команд превышен)
Отключить
моторы
write_to_flash()
(Записать данные
в энергонезависимую память)
1
Рис. 5.13. Алгоритм выбора режима работы (окончание)
1
Глава 5
140
Вначале проверяется, что у робота есть исполняемые шаги, только в этом случае
значение счетчика времени timer_counter увеличивается на 1. Напомним, что
timer_counter — это счетчик времени работы, именно его значения записываются в массив как время начала следующей команды, а участки простоя робота
отбрасываются и не воспроизводятся.
Затем проверяется, изменяется ли скорость. Если пришла для мотора новая скорость, она вместе с временем записывается в массив, а счетчик массива увеличивается на 1.
Вход в функцию обработки прерываний
Нет
Да
operating_mode == WRITE_MODE
(Если робот в режиме записи
маршрута)
3
LEFT_STEPS_counter ||
RIGHT_STEPS_counter
(Если моторы работают — есть
исполняемые шаги)
timer_counter++;
(Увеличить значения таймера
времени работы программы)
write_S_L != speed_left
(Скорость левого колеса
изменилась)
Да
array_timer_counter_left[write_counter_L] = timer_counter;
write_S_L = speed_left;
array_speed_left[write_counter_L] = int8_t(write_S_L / 10000);
(Записать в массив время прихода команды на изменение скорости и новую скорость
(число сжато делением на 10000 для уменьшения размера массива))
write_counter_L < max_i
(В массиве есть место
для новых команд)
write_counter_L++;
(Переходим к заполнению следующего элемента массива)
Процесс генерации шагов (см. главу 4)
Рис. 5.14. Алгоритм работы функции step_timer() в режиме WRITE_MODE
Учим робота самостоятельно повторять пройденный путь
141
Алгоритм обработки состояния READ_MODE в функции обработки прерывания
по таймеру step_timer() показан на рис. 5.15. Здесь проверяется текущее значение счетчика времени timer_counter на совпадение с элементом массива.
Если есть совпадение, то эмулируется поведение внешней команды управления,
а именно — из массива берется новая скорость для мотора, а счетчик элементов
массива увеличивается на 1.
3
Нет
operating_mode == READ_MODE
(Робот в режиме повтора пути)
RETURN
Да
Нет
array_timer_counter_left[write_counter_L] == timer_counter
(Пришло время изменить скорость)
Да
speed_left = int32_t(array_speed_left[write_counter_L]) * 10000;
LEFT_STEPS_counter = 2;
(Эмулируем получение команды от внешней системы (пульта) — считав команду
из массива)
Нет
write_counter_L < write_counter_Rmax
(Если в массиве еще есть команды)
Да
write_counter_L++;
(Переходим на следующий элемент массива)
timer_counter++;
(Увеличиваем счетчик времени выполнения)
Процесс генерации шагов
(см. главу 4)
Рис. 5.15. Алгоритм работы функции step_timer() в режиме READ_MODE
142
Глава 5
Особенности программы
Запись данных в энергонезависимую память
Для этого создана функция bool write_to_flash() — в ней сначала очищается
текущее хранилище, а затем последовательно записываются 4 массива и счетчики последних записанных элементов для массивов правого или левого моторов
(листинг 5.2.1).
Запись каждого массива проверяется на удачное завершение, и если хотя бы
один массив записан с ошибкой, функция возвращает false, иначе — true.
Считывание данных из энергонезависимой памяти
Для этого создана функция bool read_from_flash() — в ней последовательно
считываются 4 массива и счетчики последних записанных элементов для массивов правого или левого моторов (листинг 5.2.1). Чтение каждого массива завершается проверкой на удачное завершение, и если хотя бы один массив не считан,
функция возвращает false, иначе — true.
Световая индикация режимов записи/воспроизведения программно реализована следующим образом. Светодиоды включаются непосредственно от нажатия
соответствующих кнопок на пульте. Для организации мигания диодов служат
функции: red_blink() и gren_blue_blink().
Листинг 5.2.1. Содержимое модуля listing_5_2.ino
#include <nvs_flash.h>
// Подключаем библиотеку для работы с энергонезависимой памятью
#include <Preferences.h>
// Подключаем библиотеку для работы с адресными светодиодами
#include <Adafruit_NeoPixel.h>
// Адресный светодиод GPIO
#define NeoPixelPin 5
// Количество светодиодов
#define NUMPIXELS
1
// Создание объекта управления светодиодами
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, NeoPixelPin,
NEO_GRB + NEO_KHZ800);
// Подключаем библиотеку для работы с Bluetooth
#include <BluetoothSerial.h>
Учим робота самостоятельно повторять пройденный путь
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and
enable it
#endif
// Создаем Bluetooth-порт
BluetoothSerial SerialBT;
// Подключаем внутренние программные файлы.
#include "motorstep.h"
#include "irq_robot.h"
// Описание моторов
// Работа с прерываниями
#include "move_case.h"
// Реагирование на команды
Preferences preferences;
//=====================================
//+++++++++++++++++++++++++++++++++++++
//=====================================
void setup() {
// Старт NeoPixel
pixels.begin(); // This initializes the NeoPixel library.
// Яркий белый свет
pixels.setPixelColor(0, pixels.Color(255, 255, 255));
// Передать на светодиод
pixels.show();
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы.
motor_off();
// Инициализируем генерацию шагов моторов
timer_setup();
// Открываем Serial-порт
Serial.begin(115200);
Serial.println("Do");
}
//++++++++++++++++++++++++++++++++++++++
int32_t statisticO;
int i = 0;
#define BUTTON_GPIO 32
const int delta = 50;// Разброс
const int button_Do = 149; // Нажато воспроизведение
143
Глава 5
144
const int button_Write = 1810; // Нажата запись маршрута
void loop()
{
if (operating_mode == NO_MODE)
{
int button = analogRead(BUTTON_GPIO);
// Если нажата кнопка воспроизведения пути.
if ((button > (button_Do - delta)) && (button < (button_Do +
delta)))
{
timerAlarmDisable(Timer);
if (read_from_flash())
{
operating_mode = READ_MODE;
pixels.setPixelColor(0, pixels.Color(0, 255, 0));
pixels.show();
motor_on();
timer_counter = 0;
}
else
{
// Чтение не удалось
red_blink();
}
timerAlarmEnable(Timer);
}
else
// Если нажата кнопка записи пути.
if ((button > (button_Write - delta))
&& (button < (button_Write + delta)) )
{
// Открываем Bluetooth-порт.
SerialBT.begin("Hacker1"); // Bluetooth имя робота.
operating_mode = WRITE_MODE;
// Диод светит голубым
pixels.setPixelColor(0, pixels.Color(0, 0, 255));
pixels.show();
}
Учим робота самостоятельно повторять пройденный путь
else return;
} else if (operating_mode == WRITE_MODE)
{
// Проверяем наличие команд от смартфона
if (SerialBT.available())
{
// Читаем из буфера очередной символ
char bt_input = (char)SerialBT.read();
// Обрабатываем принятую команду
move_case(bt_input);
}
// Если нет связи со смартфоном или простаивает
// робот - отключить драйверы
if (flagTimeOff < millis())
{
if (!STEPER_EN_level)
{
timerAlarmDisable(Timer);
if (write_to_flash())
{
motor_off();
gren_blue_blink();
}
else
{
timerAlarmDisable(Timer);
red_blink();
}
timerAlarmEnable(Timer);
}
}
}
else if (operating_mode == READ_MODE)
{
if ((write_counter_Rmax <= write_counter_R)
&& (write_counter_Lmax <= write_counter_L))
{
motor_off();
pixels.setPixelColor(0, pixels.Color(255, 255, 255));
145
Глава 5
146
pixels.show();
delay(1000);
}
}
}
//===========================================================
//===========================================================
bool write_to_flash()
{
// Очищаем хранилище
//nvs_flash_erase();
preferences.begin("robo_steps", false);
preferences.clear();
if (preferences.putBytes("ar_t_c_left", array_timer_counter_left,
sizeof(array_timer_counter_left)) == 0 ) return false;
if (preferences.putBytes("ar_speed_left", array_speed_left,
sizeof(array_speed_left)) == 0 ) return false;
if (preferences.putBytes("ar_t_c_right", array_timer_counter_right,
sizeof(array_timer_counter_right)) == 0 ) return false;
if (preferences.putBytes("ar_speed_right", array_speed_right,
sizeof(array_speed_right)) == 0 ) return false;
preferences.putUShort("write_counter_L", write_counter_L);
preferences.putUShort("write_counter_R", write_counter_R);
preferences.end();
return true;
}
//===========================================================
//===========================================================
bool read_from_flash()
{
// Открываем хранилище на чтение
preferences.begin("robo_steps", false);
if (preferences.getBytes("ar_t_c_left", array_timer_counter_left,
sizeof(array_timer_counter_left)) == 0 ) return false;
if (preferences.getBytes("ar_speed_left", array_speed_left,
sizeof(array_speed_left)) == 0 ) return false;
if (preferences.getBytes("ar_t_c_right", array_timer_counter_right,
sizeof(array_timer_counter_right)) == 0 ) return false;
if (preferences.getBytes("ar_speed_right", array_speed_right,
sizeof(array_speed_right)) == 0 ) return false;
write_counter_Lmax = preferences.getUShort("write_counter_L");
Учим робота самостоятельно повторять пройденный путь
147
write_counter_Rmax = preferences.getUShort("write_counter_R");
preferences.end();
return true;
}
//===========================================================
//===========================================================
void red_blink()
{
for (int i = 0; i < 5; i++)
{
pixels.setPixelColor(0, pixels.Color(255, 0, 0));
pixels.show();
delay(100);
pixels.setPixelColor(0, pixels.Color(0, 0, 0));
pixels.show();
delay(100);
}
}
//===========================================================
//===========================================================
void gren_blue_blink()
{
for (int i = 0; i < 5; i++)
{
pixels.setPixelColor(0, pixels.Color(0, 255, 0));
pixels.show();
delay(100);
pixels.setPixelColor(0, pixels.Color(0, 0, 0));
pixels.show();
delay(100);
pixels.setPixelColor(0, pixels.Color(0, 0, 255));
pixels.show();
delay(100);
}
}
В модуль motorstep.h (листинг 5.2.2) внесены изменения максимальной скорости, минимальной скорости, ускорение также снижено.
Глава 5
148
Листинг 5.2.2. Содержимое модуля motorstep.h
(участок с изменениями скоростных характеристик)
. . .
int32_t maxSPEED = 450000; // Скорость шагов за
// ВНИМАНИЕ!!! Установка минимальной скорости менее
int32_t minSPEED = 10000; // Скорость шагов за
// 20 000; теперь это значение поделено на 1000
32-разрядных целых чисел
int32_t acceleration = 10;
int32_t accelerationL = 0;
int32_t accelerationR = 0;
. . .
100 сек
10 000 приведет к ошибке
100 сек
для вхождения в рамки
В модуль irq_robot.h добавлены 4 массива, счетчик timer_counter, описаны
флаги режимов работы. Изменения функции step_timer() представлены в алгоритме (см. рис. 5.14 и 5.15).
Листинг 5.2.3. Содержимое модуля irq_robot.h (участок с дополнениями)
. . .
const uint16_t max_i = 1000;
uint32_t timer_counter = 0;
int8_t array_speed_left[max_i];
uint32_t array_timer_counter_left[max_i];
uint16_t write_counter_L = 0;
uint16_t write_counter_Lmax = 0;
int8_t array_speed_right[max_i];
uint32_t array_timer_counter_right[max_i];
uint16_t write_counter_R = 0;
uint16_t write_counter_Rmax = 0;
#define NO_MODE 0
#define WRITE_MODE 1
#define READ_MODE 2
volatile uint8_t operating_mode = NO_MODE;
int32_t write_S_L = 0;
int32_t write_S_R = 0;
Учим робота самостоятельно повторять пройденный путь
149
//++++++++++++++++++++++++++++++++++++++++//
//======//
//++++++++++++++++++++++++++++++++++++++++//
void IRAM_ATTR step_timer() {
if (operating_mode == WRITE_MODE)
{
// Если есть неисполненные шаги, счетчик не отключаем
if (LEFT_STEPS_counter || RIGHT_STEPS_counter)
{
// Счетчик значащих тактов, в которых робот не простаивает.
timer_counter++;
}
if (write_S_L != speed_left)
{
array_timer_counter_left[write_counter_L] = timer_counter;
write_S_L = speed_left;
array_speed_left[write_counter_L] = int8_t(write_S_L / 10000);
if (write_counter_L < max_i) write_counter_L++;
}
if (write_S_R != speed_right)
{
array_timer_counter_right[write_counter_R] = timer_counter;
write_S_R = speed_right;
array_speed_right[write_counter_R] = int8_t(write_S_R
/ 10000);
if (write_counter_R < max_i) write_counter_R++;
}
}
else if (operating_mode == READ_MODE)
{
if ( array_timer_counter_left[write_counter_L] == timer_counter)
{
speed_left = int32_t(array_speed_left[write_counter_L]) * 10000;
LEFT_STEPS_counter
= 2;
if (write_counter_L < write_counter_Lmax) write_counter_L++;
}
if ( array_timer_counter_right[write_counter_R] == timer_counter)
Глава 5
150
{
speed_right = int32_t(array_speed_right[write_counter_R]) * 10000;
RIGHT_STEPS_counter = 2;
if (write_counter_R < write_counter_Rmax) write_counter_R++;
}
timer_counter++;
}
else
{
//timer_counter = 0;
return;
}
. . .
В остальном код совпадает с программой, рассмотренной в главе 4.
Ф а й ло в ы й а рх ив
Полные версии листингов, приведенных в этой главе, вы найдете в соответствующих
папках сопровождающего книгу файлового архива (см. приложение).
*
*
*
Робота с рассмотренной в этой главе программой можно выпускать на соревнования по робототехнике для прохождения лабиринтов или движения по определенной траектории — следует только пройти с ним правильно траекторию один
раз, а затем включить повторение траектории.
В следующей главе мы научимся одновременно, а не по очереди опрашивать
несколько ультразвуковых датчиков, что существенно сократит время анализа
и увеличит точность определения препятствий. На основе этой концепции мы
внесем изменения в робота и проверим полученную конструкцию при обходе
препятствий и прохождении лабиринтов.
ГЛАВА 6
АВТОНОМНОЕ ДВИЖЕНИЕ,
ОБХОД ПРЕПЯТСТВИЙ
И ПРОХОЖДЕНИЕ ЛАБИРИНТА
Подобная задача уже рассматривалась нами в книге «Мобильные роботы на
базе Arduino», но здесь мы постараемся решить ее намного проще, по возможности заменив логический анализ математическим расчетом, а вишенкой
на торте станет алгоритм измерения расстояния до препятствий, осуществляемого практически одновременно тремя или четырьмя ультразвуковыми
сонарами.
Рассмотрим маршрут с препятствиями, представляющий собой замкнутый лабиринт (рис. 6.1). В этом случае для достижения роботом финишной позиции,
отмеченной перечеркнутым кругом, ему достаточно двигаться вдоль правой или
левой стены лабиринта.
А л ь те рна т ив но е р е ше н ие
В предыдущей главе было предложено альтернативное решение подобных задач. Если
вначале проехать по лабиринту, используя программу робота с записью маршрута,
а затем повторить маршрут по памяти, задача оказывается решенной.
Если вы не имеете возможности построить сложный лабиринт, достаточно взять
большую коробку (рис. 6.2). Робот — чтобы достигнуть финиша — должен двигаться вдоль правой или левой внешней стены коробки.
В качестве сенсоров, измеряющих расстояние, мы применим недорогие ультразвуковые сонары (рис. 6.3), а чтобы робот «смотрел» не только вперед, но и в
стороны, расположим их в передней части робота под углом 30–40 градусов к
его продольной оси (рис. 6.4 и 6.5). Для крепления сонаров рекомендую использовать термоклей, тогда в случае необходимости сенсоры можно будет легко отсоединить.
Глава 6
152
Рис. 6.1. Робот обходит лабиринт вдоль правой стенки
Рис. 6.2. Робот обходит коробку вдоль правой ее стенки
Автономное движение, обход препятствий и прохождение лабиринта
Рис. 6.3. Ультразвуковой датчик US-025, выполненный на основе чипа CS100
Рис. 6.4. Расположение сонаров на роботе:
вид сверху
Рис. 6.5. Расположение сонаров на роботе: вид спереди
153
Глава 6
154
П а м я т ка по ра б оте с к ле е м
Налейте обильно клей на палубу робота по длине ребра сонара, на которое планируется его установить, установите сонар и придержите его до тех пор, пока клей не затвердеет (остынет). Повторите эту операцию для всех сонаров.
Во избежание ожогов при работе с термоклеем смачивайте кончики пальцев водой —
это предотвратит приклеивание клея к пальцам при прикосновении к нему.
Если требуется отклеить объект, приклеенный термоклеем, прогрейте место склейки
феном и осторожно разделите склеенные поверхности.
Ф а й ло в ы й а рх ив
В сопровождающем книгу файловом архиве (см. приложение) вы найдете 3D-модель
крепежа датчиков для 3D-принтера (файл Фиксатор датчиков.STL).
Ультразвуковой дальномер предназначен для определения расстояния от датчика до объекта. В основе его работы лежит принцип эхолокации — как у дельфинов или летучих мышей. Датчик состоит из передатчика, генерирующего
ультразвуковые волны, приемника, который «слушает» эхо, и обвязки для нормальной работы модуля (рис. 6.6).
Чтобы определять расстояние от робота до препятствия, нам потребуется измерять время между излученным и отраженным от препятствия ультразвуковыми
импульсами. Для одновременной обработки сигналов от нескольких ультразвуковых датчиков расстояния типа HC-SR04 и им подобных мы используем
высокие скоростные характеристики контроллера ESP32 — они позволят нам
снимать информацию с датчиков практически одновременно, что открывает
совершенно новые функциональные возможности применения этих датчиков.
Приемник
Передатчик
О
б
ъ
е
к
т
Рис. 6.6. Демонстрация прохождения звукового сигнала до объекта и прием отраженного
сигнала
Автономное движение, обход препятствий и прохождение лабиринта
155
Выберем, к примеру, датчик US-025/026, выполненный на основе чипа CS100
(см. рис. 6.3). Он способен — в отличие от традиционных датчиков HC-SR04 —
работать как с напряжением питания 5 вольт, так и с напряжением 3,3 вольта,
что освобождает нас от необходимости использовать дополнительный преобразователь напряжения на 5 вольт. Измеряемое этим датчиком расстояние:
от 2 до 600 см, точность: 0,1 см.
Готовый модуль-сонар (см. рис. 6.3) имеет 4 контакта:
• VCC — напряжение питания (будем использовать 3,3 вольта);
• GND — отрицательный контакт питания («земля»);
• Trig — сигнальный контакт, работает на прием. Полученный от контроллера 10-микросекундный импульс на этом контакте инициализирует начало
операции измерения расстояния;
• Echo — сигнальный контакт, работает на передачу. После начала операции измерения сонар выставляет на этом контакте высокое состояние, а по
окончании измерения переводит его в низкое состояние.
Последовательность выдаваемых и принимаемых сигналов показана на рис. 6.7.
Внешний контроллер генерирует сигнал на входе Trig. Сонар воспринимает его
как команду начать измерение и генерирует серию из восьми ультразвуковых
импульсов, после чего переводит сигнал на выходе Echo в высокое состояние.
Ультразвуковые импульсы отражаются от препятствия и через некоторое время
возвращаются и принимаются сонаром. После этого сигнал на выходе Echo переводится в низкое состояние. Контроллер должен измерить длительность сигнала T на выходе Echo. Зная скорость распространения звука, можно получить
расстояние в привычных единицах измерения, — например, в сантиметрах.
Входной сигнал
на Trig
Серия импульсов
Звуковой сигнал
от датчика
Длительность сигнала
Выходной сигнал
на Echo
Т
Рис. 6.7. Диаграммы выдаваемых и принимаемых сигналов
Глава 6
156
Ультразвуковой сигнал представляет собой звуковой «луч» с углом распространения 30°. Диаграмма его направленности показана на рис. 6.8 — наиболее эффективный угол измерения лежит в пределах ±15°.
0°
22,5°
22,5°
45°
45°
10’
9’
8’
7’
6’
5’
4’
3’
2’
1’
Рис. 6.8. Диаграмма распространения ультразвукового «луча»
Схемотехника
Модули US-025 можно запитать напрямую от стабилизатора на 3,3 вольта контроллера ESP32, как показано на рис. 6.9. Используйте провода с контактными
разъемами «папа-мама»: сторона провода с разъемом «мама» крепится к соответствующему выводу сонара, а с разъемом «папа» — к макетной плате.
Сигнальные входы Trig всех сонаров должны быть объединены. При этом для
запуска сонаров используется только один контакт GPIO контроллера ESP32 —
GPIO 27. Контакты GPIO для считывания длительности импульсов с разных сонаров разделены. При этом номера контактов GPIO для входа Echo не должны
быть больше 31. С учетом этого ограничения задействуем для подключения входа Echo сонаров (слева направо) к контроллеру контакты GPIO 26, 25, 18 и 17
(рис. 6.10).
Автономное движение, обход препятствий и прохождение лабиринта
157
П ояс не н ие
Контакты GPIO с номерами 0–31 представляют один регистр ввода/вывода
и могут быть считаны одной командой
чтения регистра.
Рис. 6.9. Схема питания сонаров US-025/026
Рис. 6.10. Подключение сонаров US-025/026 к контактам GPIO контроллера ESP32
Глава 6
158
Программная реализация
Алгоритм измерения расстояния
П оче м у и ме н но че т ы р е сона ра ?
Использование 4 сонаров является минимально достаточным для анализа наличия
препятствий спереди и с боков от передней части робота. Может показаться, что спереди достаточно и одного сонара, но на практике такое возможно, только если смотрящий вперед сонар перенести как можно ближе к задней части робота, — в противном случае узкая ширина луча на небольших расстояниях до препятствий приведет к
пропускам различных углов и большому количеству ошибок в действиях робота. При
этом скорость самого сканирования не увеличится.
Общий алгоритм измерения расстояния до препятствия при помощи используемого типа сонара приведен на рис. 6.11.
Для запуска измерения на вход Trig сонара подается высокий сигнал длительностью 10 микросекунд, затем на тот случай, когда отраженный сигнал не вернется, задается максимальное время ожидания отраженного сигнала в микросекундах, — это время будет храниться в переменной duration. Сравнение значения duration и текущего времени time_ ограничивает максимальное время
измерения. Если флаг startFlag начала импульса Echo еще не установлен, производится считывание с контакта Echo, в случае, если на контакте Echo появился высокий сигнал, булева переменная startFlag получает значение true,
и в переменную start_time заносится текущее время. Далее производится периодическое сканирование контакта Echo на пропадание сигнала — как только
сигнал станет низким, в stop_time заносится текущее время, и цикл завершается. Затем производится расчет расстояния до препятствия в сантиметрах —
для этого разница между временем окончания и начала сигнала Echo умножается на 0.017 (упрощенно — время умножается на скорость и делится на два:
0,017 = 0.034/2), что дает расстояние до отражающей поверхности.
С кор о с т ь з в у ка
Отметим, что скорость звука в воздухе не является постоянной величиной, — она зависит от температуры, состава и давления. Поэтому измерения обязательно будут иметь
погрешность. Если у вас есть желание изучить процесс работы с ультразвуковым сонаром подробнее, рекомендую прочитать статью В. А. Жмудь и др. «Ультразвуковой датчик измерения расстояния HC-SR04» по ссылке: http://jurnal.nips.ru/sites/default/
files/AaSI-4-2017-2.pdf.
Заметим, что стандартная функция Arduino pulseIn(Echo, value, Limit)
делает почти то же самое (измеряет длину импульса), но она останавливает
программу на время выполнения. При этом параллельное считывание данных
Автономное движение, обход препятствий и прохождение лабиринта
159
Начало
digitalWrite(Trig, true)
delayMicroseconds(10);
digitalWrite(Trig, false)
(Формируем 10-микросек. импульс)
time_ = micros();
(Запоминаем время начала измерения)
Lim = Limit * 58.8;
duration = time_ + Lim;
stop_time = 0;
(Рассчитываем максимальное время измерения)
time_ < duration
(Пока текущее время не превысило
лимит)
Нет
stop_time = duration;
(Запоминаем время
окончания цикла)
time_ = micros();
(Запоминаем текущее время)
startFlag == false
(Если не установлен флаг начала
сигнала Echo)
1
start_time = time_;
(Запоминаем время
начала сигнала Echo)
startFlag = true;
(Устанавливаем флаг
начала сигнала Echo)
Да
Да
Да
Нет
digitlRead(Echo)==true
(Если начался сигнал)
Нет
stop_time = time_;
(Запоминаем время
окончания сигнала Echo)
Нет
1
Да
startFlag == true
(Если установлен флаг начала
сигнала Echo)
long_sm = (stop_time - start_time) * 0.017;
(Рассчитываем расстояние в см)
Да
digitlRead(Echo)==false
(Если закончился сигнал)
Нет
Return -1
(Ошибка измерений)
Return long_sm
(Расстояние до препятствия в сантиметрах)
Рис. 6.11. Общий алгоритм измерения расстояния до препятствия при помощи сонаров US-025/026
160
Глава 6
с нескольких сонаров невозможно, а последовательное считывание со всех сонаров увеличивает время полного считывания. Если робот при этом еще и движется, размазывается сама картина препятствий.
Sonar() — функция одновременной работы с четырьмя
сонарами
Функция Sonar() расположена в отдельном файле parallelsonar.h совместно
с кодом поддержки (листинг 6.1, a).
В начале файла parallelsonar.h заданы пять констант с номерами контактов GPIO
контроллера:
• SONAR_TRIG — номер контакта GPIO, соединенного с контактом Trig всех
сонаров;
• SONAR_RIGHT — номер контакта GPIO, соединенного с контактом Echo правого сонара;
• SONAR_RIGHT_FRONT — номер контакта GPIO, соединенного с контактом
Echo правого переднего сонара;
• SONAR_LEFT_FRONT — номер контакта GPIO, соединенного с контактом Echo
левого переднего сонара;
• SONAR_LEFT — номер контакта GPIO, соединенного с контактом Echo левого сонара.
Далее в этом файле описана структура sruct_sonar_data, содержащая:
• start_time — 32-разрядное беззнаковое целое число, время начала импульса;
• start_time — 32-разрядное беззнаковое целое число, время окончания
импульса;
• long_sm — расстояние до препятствия в сантиметрах (число с плавающей
точкой);
• GPIO — 32-разрядное число, которое содержит 1 только в одном двоичном
разряде, соответствующем номеру контакта GPIO, соединенного с контактом Echo определенного сонара в регистре ввода/вывода GPIO_IN_REG;
• startFlag — выставление в true этого булева значения обозначает, что на
контакт GPIO от контакта Echo сонара поступил высокий сигнал;
• stopFlag — выставление в true этого булева значения обозначает, что на
контакт GPIO от контакта Echo сонара поступил низкий сигнал.
Далее создается массив long_sm из 4 элементов, каждый из которых имеет
структуру sruct_sonar_data. Этот массив служит для цикличной обработки
Автономное движение, обход препятствий и прохождение лабиринта
161
четырех сонаров. При использовании большего числа сонаров принципиальные изменения не потребуются.
Объявленная переменная time_ служит для хранения текущего времени,
а duration — времени принудительного окончания измерения.
Функция Sonar_init() служит для заполнения элементов long_sm[i].GPIO
масками, содержащими 1 только в одном двоичном разряде, соответствующем
номеру контакта GPIO, соединенного с контактом Echo определенного сонара
в регистре ввода/вывода GPIO_IN_REG. Также в функции задается режим работы используемых контактов GPIO.
Функция void Sonar(uint32_t Limit = 40) реализует алгоритм, показанный на рис. 6.11. Ее особенностью является использование быстрой операции
чтения данных с контактов GPIO, соединенных с контактами на Echo всех четырех сонарах: REG_READ(GPIO_IN_REG). В переменной registr_in хранятся
возвращаемые функцией REG_READ(GPIO_IN_REG) значения GPIO с номерами
от 0 до 31, настроенные на чтение внешних сигналов.
Нам остается только выделить нужные разряды и проанализировать. Например:
(registr_in & long_sm[4].GPIO) будет не равен нулю, если в GPIO 26
(SONAR_LEFT) содержится 1, — или другими словами, на этот контакт контроллера подано высокое напряжение. Когда все сигналы измерены (или закончилось выделенное время), производится расчет расстояния в сантиметрах
до препятствия для каждого сонара. По окончании работы функции Sonar()
в основной программе в массив структур long_sm, в элементах с long_sm[0].
long_sm по long_sm[3].long_sm будут сохранены расстояния, измеренные при
помощи соответствующего сонара (слева направо).
Листинг 6.1, а. Файл parallelsonar.h (работа с сонарами)
// Номера GPIO-сонаров.
#define SONAR_TRIG 27
#define SONAR_RIGHT 17
#define SONAR_RIGHT_FRONT 18
#define SONAR_LEFT_FRONT 25
#define SONAR_LEFT 26
struct sruct_sonar_data
{
Uint32_t start_time = 0;
Uint32_t stop_time = 0;
double long_sm = 0;
162
Глава 6
uint32_t GPIO;
bool startFlag = false;
bool stopFlag = false;
};
sruct_sonar_data long_sm[4];
uint32_t time_, duration;
//=Режим пинов/портов ===============================
void Sonar_init()
{
long_sm[0].GPIO = 1 << SONAR_LEFT;
long_sm[1].GPIO = 1 << SONAR_LEFT_FRONT;
long_sm[2].GPIO = 1 << SONAR_RIGHT_FRONT;
long_sm[3].GPIO = 1 << SONAR_RIGHT;
// Задаем режимы работы пинов.
pinMode(SONAR_TRIG, OUTPUT);
for (int i = 0; i < 4; i++)
pinMode(long_sm[i].GPIO, INPUT);
}
//= Измерение расстояния ============================
void Sonar(uint32_t Limit = 40)
{
uint32_t registr_in;
for (int i = 0; i < 4; i++)
{
long_sm[i].start_time = 0;
long_sm[i].stop_time = 0;
long_sm[i].long_sm = Limit;
long_sm[i].startFlag = false;
long_sm[i].stopFlag = false;
}
// Переводим лимит из сантиметров в относительные величины
(микросек.)
// При известной скорости звука, расстояние прямо пропорционально
// времени, потраченному звуковой волной
uint64_t Lim = Limit * 58.8;
// Генерируем импульс
digitalWrite(SONAR_TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(SONAR_TRIG, LOW);
Автономное движение, обход препятствий и прохождение лабиринта
163
// Ждем отражения импульса.
// Пересчитываем время в расстояние (по скорости звука).
int all_close = 0;
time_ = micros();
duration = time_ + Lim;
while ((time_ < duration) && (all_close < 4))
{
time_ = micros();
registr_in = REG_READ(GPIO_IN_REG);
for (int i = 0; i < 4; i++)
{
if (long_sm[i].stopFlag == false)
{
if (long_sm[i].startFlag == false)
{
if ((registr_in & long_sm[i].GPIO) != 0) {
long_sm[i].start_time = time_;
long_sm[i].startFlag = true;
}
}
else
{
if ((registr_in & long_sm[i].GPIO) == 0)
{
long_sm[i].stop_time = time_;
long_sm[i].stopFlag = true;
all_close++;
}
}
}
}
}
// Если импульс нормально начался и завершился
// в заданное время
for (int i = 0; i < 4; i++)
if (long_sm[i].stopFlag)
{
long_sm[i].long_sm = double(long_sm[i].stop_time - long_sm[i].
start_time) * 0.017;
}
}
164
Глава 6
Основная программа
Алгоритм основной программы (рис. 6.12) содержит всего один блок логического анализа — все остальные обеспечивают пропорциональную реакцию колес на показания датчиков. Рассмотрим этот механизм подробнее. В примере
программы используется обход по правой стене лабиринта. В этом случае робот
должен двигаться вдоль стены на заданном от нее расстоянии.
Максимальная дистанция зондирования задается константой: SCAN_MAX_DIST
(40 см). Дадим роботу задание двигаться на удалении, равном половине от SCAN_
MAX_DIST. Это значение (20 см) будем хранить в переменной half_scan_res:
half_scan_res= SCAN_MAX_DIST /2
Пусть робот движется по прямой со скоростью Movation1, равной половине от
максимальной maxSPEED:
Movation1 = maxSPEED / 2
Параметры half_scan_res и Movation1 можно менять в процессе экспериментов.
При движении по прямой на заданном расстоянии от стены оба колеса имеют
одинаковую скорость Movation1.
Если правый сонар обнаруживает, что робот удалился от стены, его надо вернуть на место: скорость левого колеса — увеличим, скорость правого колеса —
уменьшим на значение, которое будем хранить в переменной Rotation1.
Здесь Rotation1 — поворотная составляющая скорости колеса, она влияет на
расчетную скорость правого и левого колеса по-разному (для левого колеса она
идет со знаком минус).
Если робот движется на положенном расстоянии от стены, то Rotation1 = 0.
Если робот отклоняется от стены более чем на half_scan_res (рис. 6.13, а), то
величина Rotation1 становится отрицательной, что приводит к уменьшению
скорости правого колеса и увеличению скорости левого, и робот возвращается
на свою траекторию — подворачивает вправо (рис. 6.13, б).
Если робот приблизился к стене больше чем требуется, величина Rotation1
становится положительной, и робот начинает подворачивать влево (поворачивание происходит с одновременным движением вперед):
RighPower = Rotation1 + Movation1;
LeftPower = -Rotation1 + Movation1;
Автономное движение, обход препятствий и прохождение лабиринта
165
Значение Rotation1 будет вычисляться по следующей формуле:
Rotation1 = (half_scan_res - long_sm[3].long_sm) * Krot1;
Здесь: long_sm[3].long_sm — расстояние, измеренное правым сонаром. Когда
это расстояние равно заданному, Rotation1 равняется нулю. Чем больше отклонение long_sm[3].long_sm от half_scan_res, тем больше Rotation1.
Krot1 — эмпирический коэффициент пропорциональности. Он может подбираться экспериментально, но мы примем его равным 1/10 максимальной скорости колеса робота.
Таким образом, робот будет двигаться вдоль стены до тех пор, пока не встретится с ситуацией, приведенной на рис. 6.14, а. Здесь сонар, расположенный
справа, покажет, что расстояние до стены в норме, и робот, если не проводить
дополнительных измерений и корректировок, продолжит двигаться вперед, что
приведет к столкновению со стеной. Для исключения подобной ситуации нужно
выполнять дополнительный анализ по сонарам, расположенным спереди робота. В нашем случае таких сонаров пара, и если на одном из них расстояние до
препятствия менее критичного, то робот переходит в режим разворота на месте
(рис. 6.14, б):
RighPower = maxSPEED / 2;
LeftPower = -maxSPEED / 2;
Далее функция set_new_speed(RighPower, LeftPower) выполняет минимальные действия, чтобы рассчитанная скорость была принята в качестве скорости движения.
В алгоритме (см. рис. 6.12) и программе (листинг 6.1, б) присутствует блок задания значений, которые в дальнейшем в программе не изменяются, но могут
быть изменены при отладке робота (мы уже к ним обращались ранее, но не грех
и повторить):
• SCAN_MAX_DIST — задает максимальную дистанцию измерения расстояния
до препятствия сонаром;
• stop_DIST — дистанция останова и разворота. Если передние сонары показывают меньше, чем stop_DIST, робот переходит в состояние разворота
на месте;
• Krot1 — эмпирический коэффициент пропорциональности, используемый
для вычисления Rotation1;
• Movation1 — скорость движения по прямой, равна половине максимальной;
• half_scan_res — заданное расстояние робота от стены, принимается равным половине от величины SCAN_MAX_DIST.
Глава 6
166
Заметим, что в нашем алгоритме задействованы только три сонара — сонар, расположенный слева, хоть и измеряет расстояние, но в формировании движения
робота не участвует. Так что если у вас всего в наличии только три сонара, и вы
не планируете модернизировать программу, то достаточно внести небольшие
изменения в раздел Номера GPIO сонаров файла parallelsonar.h. Так, в строке
#define SONAR_LEFT 26 вместо номера GPIO 26, который связан с левым сонаром, установить номер GPIO, задействованного на переднем слева сонаре:
Начало
//Дистанция сканирования
SCAN_MAX_DIST = 40;
//Дистанция «стена спереди»
stop_DIST = 15;
//Коэффициент поворота
Krot1 = maxSPEED / 10.0;
//Коэффициент движения вперед
Movation1 = maxSPEED / 2;
// Половина дистанции сканирования
half_scan_res = SCAN_MAX_DIST / 2;
Запуск функции
Sonar(SCAN_MAX_DIST)
long_sm[2].long_sm > stop_DIST && long_sm[1].long_sm > stop_DIST
Если на передних датчиках нет препятствия ближе, чем stop_DIST
Да
Нет
//Если впереди препятствие,
начать поворот влево на месте
//Расчет скоростей на колеса
RighPower = maxSPEED / 2;
LeftPower = -maxSPEED / 2;
//Расчет поворота
Rotation1 = -(long_sm[3].long_sm - half_scan_res) * Krot1;
//Расчет скоростей на колеса
RighPower = Rotation1 + Movation1;
LeftPower = -Rotation1 + Movation1;
Запуск функции
set_new_speed(RighPower, LeftPower);
Рис. 6.12. Алгоритм программы обхода лабиринта по правой стене
Автономное движение, обход препятствий и прохождение лабиринта
167
// Номера GPIO-сонаров.
#define SONAR_TRIG 27
#define SONAR_RIGHT 17
#define SONAR_RIGHT_FRONT 18
#define SONAR_LEFT_FRONT 25
#define SONAR_LEFT 25
Теперь передний слева сонар будет проверяться дважды, что не критично.
Программа, приведенная в листинге 16.1, б полностью повторяет рассмотренный алгоритм.
а
б
Рис. 6.13. Робот при движении вдоль стены: а — отклоняется от стены более чем
на half_scan_res; б — возвращается на свою траекторию
Глава 6
168
а
б
Рис. 6.14. Робот при движении вдоль стены: а — встретил препятствие спереди; б — совершил
разворот на месте
Автономное движение, обход препятствий и прохождение лабиринта
169
Листинг 6.1, б. Файл listing_6_1.ino (основная программа)
// Подключаем внутренние программные файлы.
// Подключаем внутренние программные файлы.
#include "motorstep.h"
// Описание моторов
#include "irq_robot.h"
// Работа с прерываниями
#include "parallelsonar.h"
//=====================================
//+++++++++++++++++++++++++++++++++++++
//=====================================
void setup() {
Sonar_init();
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы.
motor_on();
// Инициализируем генерацию шагов моторов
timer_setup();
// Открываем Serial-порт
Serial.begin(115200);
}
//++++++++++++++++++++++++++++++++++++++
// Дистанция сканирования
const int32_t SCAN_MAX_DIST = 40;
// Дистанция останова - разворота
const int32_t stop_DIST = 15;
// Коэффициент поворота
const
int32_t Krot1 = maxSPEED / 10;
// Коэффициент движения вперед
const
int32_t Movation1 = maxSPEED / 2; // / 40;
const
int32_t half_scan_res = SCAN_MAX_DIST / 2;
int32_t RighPower, LeftPower, Rotation1;
void loop()
{
Sonar(SCAN_MAX_DIST);
if (long_sm[2].long_sm > stop_DIST && long_sm[1].long_sm > stop_DIST)
Глава 6
170
{
Rotation1 = -(long_sm[3].long_sm - half_scan_res) * Krot1;
RighPower = Rotation1 + Movation1;
LeftPower = -Rotation1 + Movation1;
}
else
{
RighPower = maxSPEED / 2;
LeftPower = -maxSPEED / 2;
}
set_new_speed(RighPower, LeftPower);
delay(50);
}
//=============================================
void set_new_speed(int32_t s_r, int32_t s_l)
{
speed_right = constrain(s_r, -maxSPEED, maxSPEED);
speed_left = constrain(s_l, -maxSPEED, maxSPEED);
// если требуемая скорость не равна с текущей.
if (speed_last_L != speed_left)
{
LEFT_STEPS_counter
= 2;
}
// если требуемая скорость не равна с текущей.
if (speed_last_R != speed_right)
{
RIGHT_STEPS_counter
= 2;
}
}
Ф а й ло в ы й а рх ив
Полные версии листингов, приведенных в этой главе, вы найдете в соответствующей
папке сопровождающего книгу файлового архива (см. приложение).
Автономное движение, обход препятствий и прохождение лабиринта
171
Самостоятельное задание
• Используя кнопки управления роботом, рассмотренные в главе 5, измените
программу таким образом, чтобы робот начинал обход только после нажатия одной из них.
• Используя кнопки управления роботом, рассмотренные в главе 5, измените
программу таким образом, чтобы робот начинал обход по правой или левой
стене в зависимости от нажатой кнопки.
Выводы
Мы научили робота обходить препятствия лабиринта, используя быстрый алгоритм измерения расстояния. Скорость измерения значительно не изменится,
если таких сонаров будет не четыре, а больше, что важно.
Надеемся, что материал этой главы был для вас интересен и не сложен, а следующую главу мы посвятим роботам телеприсутствия — установим на робота
камеру и будем управлять роботом, используя компьютерные сети Wi-Fi, как из
внутренней сети, так и из любой точки планеты через Интернет.
ГЛАВА 7
РОБОТ ТЕЛЕПРИСУТСТВИЯ
Роботы телеприсутствия позволяют человеку удаленно наблюдать за событиями, происходящими вокруг роботов, и определенным образом влиять на них.
Наша цель — создать дистанционно управляемого мобильного робота с передающей камерой. Рассмотрим вариант создания подобного робота с управлением
от первого лица — установим камеру на самого робота.
Смартфон на роботе в роли камеры
Проще всего установить на робота смартфон под управлением ОС Android
(рис. 7.1).
Нам, соответственно, потребуется смартфон, работающий на ОС Android версии
не ниже 5. Состояние смартфона не принципиально, главное — наличие рабочей камеры, батареи и возможность установить и запустить программу, которая
будет всем этим управлять.
Рис. 7.1. Вариант установки робота смартфона в роли камеры
Глава 7
174
Для передачи нам информации от камеры робота мы воспользуемся каналом
Wi-Fi — это значит, что в зоне обитания робота должна функционировать сеть
Wi-Fi, и вы должны иметь возможность подключить к этой сети установленный
на робота смартфон.
Каким образом будет установлен смартфон на робота — не столь принципиально: на рис. 7.1, например, показаны три фиксирующих смартфон элемента, можно также использовать фиксатор от селфи-палки.
Для управления камерой смартфона мы применим программу VideoRobot от
компании NECO (рис. 7.2, а) — найдите ее в Google Play и выполните установку.
Если программы VideoRobot в Google Play почему-то не окажется, обратитесь на
сайт разработчика (https://neco-desarrollo.es) и скачайте ее оттуда. При этом
для установки приложения на смартфон вам потребуется в настройках безопасности смартфона разрешить установку приложений из сторонних источников.
Ф а й ло в ы й а рх ив
Программу VideoRobot от компании NECO можно скачать на Play Маркете,
с сайта издательства или по ссылке
https://neco-desarrollo.es/wp-content/uploads/2019/10/video_robot_v_1.1.6-2.rar
Запустите на смартфоне приложение VideoRobot. Если смартфон подключен
к сети Wi-Fi, вы увидите главное окно программы с индексацией IP-адреса
а
б
Рис. 7.2. Программа VideoRobot: а — установка; б — окно программы с индексацией IP-адреса
смартфона
Робот телеприсутствия
175
смартфона в этой сети (рис. 7.2, б), — запишите этот адрес. Откройте настройки
программы (рис. 7.3, а), задайте порт IP-соединения (рис. 7.3, б) и настройте
качество передаваемого изображения. Не гонитесь за высокими качеством изображения и частотой кадров — это может привести к тому, что изображение будет транслироваться со значительными задержками. Рекомендуется разрешение
камеры — 320×240 пикселов (рис. 7.3, в), качество изображения — 300000 bit
(рис. 7.4, а), частота — 15 кадров в секунду (рис. 7.4, б).
а
б
в
Рис. 7.3. Программа VideoRobot: а — главное окно настроек;
б — установка порта IP-соединения; в — установка разрешения камеры
а
б
Рис. 7.4. Программа VideoRobot: а — установка качества изображения;
б — выбор частоты кадров
Глава 7
176
Настройка смартфона управления
Для управления роботом нам понадобится и второй смартфон — на него будет транслироваться изображение с камеры робота. Требования к смартфону
удаленного управления не столь жесткие — он может работать на ОС Android
4-й версии, но также должен быть подключен к сети Wi-Fi.
Для работы с этим смартфоном мы применим программу VideoControl от компании NECO (рис. 7.5, а). Найдите ее в Google Play и выполните установку.
Главное окно программы VideoControl показано на рис. 7.5, б. Центральную его
часть занимает принимаемое видеоизображение и два экранных джойстика, а в
верхней части расположены кнопки управления — четыре левые кнопки управляют удаленным смартфоном, установленным на роботе: включение фонарика
(1), включение Bluetooth (2), подключение удаленного смартфона к роботу по
Bluetooth (3), смена камеры для трансляции — передняя/задняя (4). Далее идет
кнопка настройки связи с удаленным смартфоном (5) и кнопка вызова помощи
по обеим управляющим смартфонами программам (6).
Для подключения к смартфону, установленному на роботе, нажмите в главном окне программы кнопку настройки связи с этим смартфоном (кнопка 5
1
а
б
2
3
4
5
6
в
Рис. 7.5. Программа VideoControl: а — установка; б — главное окно и кнопки управления;
в — настройки IP-адреса и порта
Робот телеприсутствия
а
177
б
в
Рис. 7.6. Настройка программы VideoControl: а — установка внешнего IP-адреса; б — установка
внутреннего IP-адреса; в — задание порта
на рис. 7.5, б) и в открывшемся окне настроек (рис. 7.5, в) установите флажок
Включить внешний адрес IP — без этого не получится связать смартфоны.
Затем поочередно пройдите из этого окна по ссылкам Внешний адрес IP,
Внутренний адрес IP и Порт и введите в поля Внешний адрес IP (рис. 7.6, а)
и Внутренний адрес IP (рис. 7.6, б) один и тот же IP-адрес — тот, который отображался на смартфоне робота (см. рис. 7.2, б), а в поле ввода порта (рис. 7.6, в) —
то же число, что было введено на смартфоне робота, — у нас это число 5543.
Соединение смартфонов
В главном окне установленного на роботе смартфона (с программой VideoRobot) нажмите кнопку
Start — смартфон перейдет в режим соединения.
В главном окне смартфона удаленного управления
(с программой VideoControl) нажмите центральную
кнопку, похожую на знак воспроизведения, — черный экран сменится на транслируемое со смартфона
робота изображение (рис. 7.7).
Изменяя положение робота со смартфоном, оцените
задержку и качество изображения, попробуйте изменять видеорежимы, выберите из них оптимальный.
Что ж, смартфоны соединены, но пока они никак не
управляют нашим роботом. Попробуем разобраться
в этом вопросе.
Рис. 7.7. Изображение,
транслируемое со смартфона
робота
Глава 7
178
Соединение смартфона на роботе с роботом
Соединение установленного на роботе смартфона (под управлением программы
VideoRobot) с роботом мы будем осуществлять по каналу Bluetooth — аналогично тому, как мы это делали главе 4. Для этого зарегистрируйте в смартфоне
робота как устройство, а затем в настройках соединения Bluetooth программы
VideoRobot (рис. 7.8) внесите имя контроллера ESP32 нашего робота (вспомним, что в главе 4 мы дали ему имя Hacker1).
Однако, используя программу из главы 4, управлять роботом с помощью нашего
смартфона не получится, поскольку в программе VideoRobot задействован немного иной командный протокол.
В соответствии с ним, при использовании экранных джойстиков (см. рис. 7.7)
левый служит для передачи команд на перемещения робота, а правый — для
вращения головы робота. Голова
нашего робота неподвижна, а вот
команды, генерируемые при использовании левого джойстика, мы должны принять и обработать. Фактически
смартфон при нажатии левого джойстика генерирует последовательности
латинских символов: A — вперед, E —
назад, C — влево и B — вправо. При
этом символы остановки не генерируются, и нам следует самостоятельно обработать ситуацию, когда никакие команды на робота не приходят,
то есть остановить его.
Основная программа (листинг 7.1.1),
помимо приема командных символов
по Bluetooth, содержит анализ времени приема последней команды, и если
это время больше установленного, то
генерируется команда останова для
робота. Это единственное отличие
этого листинга от листинга 4.2.4.
Рис. 7.8. Настройки соединения Bluetooth
программы VideoRobot
Робот телеприсутствия
179
Листинг 7.1.1. Основная программа управления роботом телеприсутствия
(файл listing_7_1.ino)
// Создаем Bluetooth-порт
BluetoothSerial SerialBT;
// Подключаем внутренние программные файлы.
#include "motorstep.h"
// Описание моторов
#include "irq_robot.h"
// Работа с прерываниями
#include "move_case.h"
// Реагирование на команды
//=====================================
//+++++++++++++++++++++++++++++++++++++
//=====================================
void setup() {
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы.
motor_off();
// Инициализируем генерацию шагов моторов
timer_setup();
// Открываем Serial-порт
Serial.begin(115200);
// Открываем Bluetooth-порт.
SerialBT.begin("Hacker1"); // Bluetooth имя робота.
Serial.println("Do");
}
//++++++++++++++++++++++++++++++++++++++
int32_t statisticO;
int i = 0;
void loop()
{
// Проверяем наличие команд от смартфона
if (SerialBT.available())
{
// Читаем из буфера очередной символ
char bt_input = (char)SerialBT.read();
// Обрабатываем принятую команду
move_case(bt_input);
Глава 7
180
Serial.println(ESP.getFreeHeap());
}
else // Если нет новых команд, остановиться, отключить мотор
{
if (flagTimeStop < millis())
{
char bt_input = 'S';
move_case(bt_input);
}
if (flagTimeOff < millis()) motor_off();
}
}
Файл move_case.h (листинг 7.1.2) — по сравнению с таким же файлом из
главы 4 — претерпел изменения, получив дополнительную переменную
flagTimeStop, служащую для хранения времени остановки робота по отсутствию команд. В нем также (в блоке switch (bt_input)) изменены символы,
отвечающие за движения.
Листинг 7.1.2. Модуль move_case.h (анализ принятых команд)
unsigned long flagTimeOff;
unsigned long flagTimeStop;
//==============================================
void move_case(char &bt_input)
{
if (bt_input != 'S')
{
flagTimeOff = millis() + 2500;
flagTimeStop = millis() + 200;
motor_on();
}
switch (bt_input) {
// Вперед
case 'A':
//
speed_left = maxSPEED;
speed_right = maxSPEED;
break;
Робот телеприсутствия
181
// Назад
case 'E':
speed_left = -maxSPEED;
speed_right = -maxSPEED;
break;
// Влево
case 'C':
speed_left = -maxSPEED;
speed_right = maxSPEED;
break;
// Вправо
case 'B':
speed_left
= maxSPEED;
speed_right = -maxSPEED;
break;
// Стоп
case 'S':
speed_left
= 0;
speed_right = 0;
break;
}
// Если требуемая скорость не равна с текущей.
if (speed_last_L != speed_left)
{
LEFT_STEPS_counter
= 2;
}
// Если требуемая скорость не равна с текущей.
if (speed_last_R != speed_right)
{
RIGHT_STEPS_counter
= 2;
}
}
Ф а й ло в ы й а рх ив
Полные версии листингов, приведенных в этой главе, вы найдете в соответствующих
папках сопровождающего книгу файлового архива (см. приложение).
Глава 7
182
Управляем роботом через Интернет
Теперь мы можем дистанционно управлять роботом в пределах общей сети WiFi, а для того чтобы реализовать удаленное управление через Интернет, потребуется обеспечить дополнительные условия:
• ваша сеть Wi-Fi должна иметь выход в Интернет;
• внешний IP-адрес роутера выхода в Интернет должен быть статическим
(этим параметром управляет провайдер);
• на роутере, через который осуществляется выход в Интернет, нужно выполнить операцию, называемую «проброс портов»;
• на смартфоне с программой VideoControl, который подключен к Интернету
посредством сотовой связи, скорректировать IP-адрес и порт.
В качестве примера приведем этапы настройки роутера D-Link с внешним IPадресом 176.197.141.15 и внутренним IP-адресом 196.168.1.1:
1. В адресной строке браузера вводим внутренний адрес роутера: 196.168.1.1,
попадаем в окно авторизации (рис. 7.9) и вводим пароль администратора.
2. В меню Межсетевой экран выбираем пункт Виртуальные серверы и нажимаем в правой части окна на значок
(рис. 7.10).
+
3. В открывшемся окне ввода параметров (рис. 7.11) заносим параметры нового виртуального сервера: имя, IP-адрес и порт смартфона VideoRobot, заполняем также поле Внешний порт (выбирается произвольно, но больше
8000).
4. Смартфон с установленной программой VideoControl отключаем от сети WiFi, подключаем к сети передачи данных сотовой связи (рекомендуется для
улучшения качества связи подойти к окну или выйти из помещения).
Рис. 7.9. Настройка роутера: вход по паролю администратора
Робот телеприсутствия
183
5. Изменяем на этом смартфоне IP-адрес соединения на внешний IP-адрес роутера, а порт — на установленный в роутере внешний порт (рис. 7.12).
6. Не забываем нажать кнопку Старт в программе VideoRobot на смартфоне
робота.
После этого робот станет виден из любой точки Интернета и будет доступен для
управления.
Рис. 7.10. Настройка роутера: добавление виртуального сервера
Рис. 7.11. Настройка роутера: создание разрешения для IP-адреса 192.168.1.205 и задание
внешнего порта 8085
Глава 7
184
а
б
Рис. 7.12. Настройка программы VideoControl на смартфоне управления: в качестве IP-адреса
используется внешний IP-адрес роутера (а), а в качестве порта — порт, настроенный в роутере (б)
Модуль ESP32-CAM на роботе в роли камеры
Смартфон — это не единственное устройство, которое можно использовать в качестве камеры на роботе. Существует также модуль ESP32-CAM, который основан на контроллере ESP32 и оснащен встроенной камерой (рис. 7.13).
Помимо камеры, модуль ESP32-CAM несет на борту слот под карту microSD и
яркий светодиод для освещения места съемки. К сожалению, разработчики не
оснастили модуль конвертером USB-UART для подключения к USB-порту компьютера, что осложняет подготовку его к программированию. Поэтому нам
потребуется отдельный модуль — конвертер USB-UART (рис. 7.14). Такой конвертер при подключении его к ПК и установке подходящих драйверов создает
последовательный порт COM-UART.
Чтобы программировать ESP32-CAM, соедините его с конвертером, как показано на рис. 7.15. Обратите внимание: прошивка модуля становится возможной, только если контакт IO0 закорочен на «землю» (GND), а при нормальной
работе это соединение должно быть разорвано. Соединение портов UART осуществляется перекрестно: TXD (передача) на UOR (чтение) и RXD (чтение) на
UOT (передача).
Робот телеприсутствия
185
а
GND
U0TXD
U0RXD
3.3V/5V Output
GND
CSI_MCLK
IO16
3.3V
33,8 mm
HS2_DATA1
HS2_DATA0
HS2_CLK
HS2_CMD
HS2_DATA3
HS2_DATA2
GND
5V
40 mm
2,54 mm
ANT
6,3 mm
27 mm
б
2,07 mm
22,06 mm
в
27 mm
Рис. 7.13. Модуль ESP32-CAM: а — общий вид в сборе с камерой; б — плата модуля;
в — распиновка
Рис. 7.14. Конвертер USB-UART
Глава 7
186
Рис. 7.15. Соединение конвертера USB-UART и модуля ESP32-CAM для прошивки модуля
Рис. 7.16. Выбор платы AI Thinker ESP32-CAM
Рис. 7.17. Выбор тестовой программы CameraWebServer
Робот телеприсутствия
187
Полученным последовательным соединением можно пользоваться не только
для прошивки модуля ESP32-CAM, но и для передачи и приема данных между
ESP32-CAM и компьютером.
Теперь настроим среду Arduino IDE, выбрав для работы плату AI Thinker ESP32CAM (рис. 7.16), а для использования ее в качестве камеры выберем в меню примеров: ESP32 | Camera | CameraWebServer (рис. 7.17).
В выбранный нами пример CameraWebServer внесем пару исправлений
(рис. 7.18):
• закомментируем строку 10 и удалим комментарий в строке 14 — тем самым
мы выберем настройки для нашего модуля;
• в строки 18 и 19 вместо звездочек внесем имя нашей сети Wi-Fi и пароль
доступа к сети.
И сохраним исправленный пример под новым именем: CameraWebServerEx.ino,
где Ex — означает Expanded (расширенная версия).
Все. Программа готова к загрузке в модуль.
Ф а й ло в ы й а рх ив
Полные версии листингов и файлов, относящихся к управлению камерой модуля
ESP32-CAM, вы найдете в папке CameraWebServerEx сопровождающего книгу файлового архива (см. приложение).
а
б
Рис. 7.18. Настройка тестовой программы CameraWebServer: а — исходный текст;
б — исправленный текст
Глава 7
188
Рис. 7.19. Программа CameraWebServer соединяет модуль ESP32-CAM с сетью Wi-Fi
и возвращает IP-адрес и порт соединения
Перед загрузкой программы убедитесь, что контакт IO0 закорочен на GND,
иначе прошивка не получится, выберите порт, соответствующий плате конвертера, и загрузите программу. Компиляция и загрузка проходят весьма медленно,
что связано с большим объемом программы.
После успешной загрузки откройте окно Монитора порта и перезагрузите модуль разрывом цепи питания или нажатием на кнопку RST с обратной стороны
модуля (если ESP32-CAM стоит на макетной плате, кнопка RST может оказаться
недоступной).
В Мониторе порта (рис. 7.20) сначала высветится служебная информация по
загрузке модуля, а затем начнут бежать точки до тех пор, пока ESP32-CAM не
подключится к сети Wi-Fi. После успешного соединения мы получим IP-адрес и
порт, на котором будет виден наш модуль в качестве веб-сервера.
П рич и н ы отс у тс тв и я со е д и не н и я с Wi - Fi
Если соединение долго не происходит, проверьте правильность имени сети и пароля.
Если эти данные введены правильно, но соединения нет, возможно, виноваты дополнительные настройки сети. Во время экспериментов в нашей сети модуль ESP32-CAM
очень долго соединялся с Wi-Fi — пока не было принято решение программно отключить сетевой протокол 5 ГГц в точке доступа Wi-Fi (мы использовали точку доступа
DIR-825ACG1). После этого все отлично работало. Эксперименты также показали, что
без отключения 5 ГГц все также работало, если установить на модуль внешнюю антенну и касаться антенны рукой. С точками, работающими только в одном диапазоне
2,5 Ггц, проблем соединения не наблюдалось.
Робот телеприсутствия
189
После успешного подключения ESP32-CAM к сети Wi-Fi можно переходить на
компьютер, планшет или смартфон, подключенный к этой сети (компьютер при
этом может быть подключен к сети по кабелю). Запускаем браузер и в его адресную строку вводим полученный при соединении с сетью IP-адрес. В окне браузера буден показана страница сайта, схожая с рис. 7.20, но без картинки справа. Нажатие кнопки Start Stream (кнопка внизу страницы по центру) запустит
трансляцию с камеры модуля, а кнопка Start Stream при этом превратится в
Stop Stream.
Рис. 7.20. Подключаемся к камере через браузер
190
Глава 7
Модернизация робота
Научившись работать с камерой, дооснастим теперь этой камерой нашего робота. Заметим, что алгоритмы обработки изображений весьма требовательны
к ресурсам, поэтому совместить управление шаговыми моторами и работу с камерой в рамках одного модуля ESP32-CAM затруднительно. Гораздо проще подключить модуль ESP32-CAM по последовательному интерфейсу к уже готовому
роботу, построенному на основе ESP32. Фактически, модуль ESP32-CAM будет
выступать в роли смартфона на роботе из первого раздела этой главы.
Изменения программы
Сохраним программу CameraWebServerEx под новым именем: CameraRobot и,
чтобы выполнить задуманную модернизацию, внесем в нее следующие изменения/дополнения:
• изменим интерфейс сайта, удалив из него лишние кнопки и добавив кнопки
управления роботом;
• создадим обработку нажатия кнопок для движения роботом и передачу команд управления основному контроллеру управления роботом.
В файле camera_index.h нашего проекта содержится массив с именем index_
ov2640_html_gz, заполненный числами в шестнадцатеричном коде (рис. 7.21).
Это сжатый HTML-документ (главная страница сайта управления камерой).
Рис. 7.21. Сжатый и помещенный в массив HTML-документ (главная страница сайта камеры)
Робот телеприсутствия
191
Для его корректировки нам потребуется двоичный редактор — подойдет
бесплатный двоичный редактор HxD.
Копируем массив в обычный текстовый редактор (у нас это Notepad++)
и удаляем автозаменой лишние символы: пробелы, запятые, группы 0х
(рис. 7.22, а). Полученные последовательности копируем и вставляем в
пустой документ двоичного редактора
HxD (рис. 7.22, б). Сохраняем полученный документ на диск в файл под
именем index_ov2640.html.gz. Теперь
распакуем архив — для этого мы использовали бесплатную версию архиватора RAR (рис. 7.23) — и получим код страницы сайта камеры (на
рис. 7.24 этот код показан в текстовом
редакторе Notepad++). Для большей
убедительности страницу можно открыть в браузере.
а
б
Рис. 7.22. Редактирование двоичных файлов: а — удаление лишних символов в редакторе
Notepad++; б — сохранение полученных последовательностей в документе двоичного
редактора HxD
Глава 7
192
Рис. 7.23. Извлечение из архива файла index_ov2640.html в архиваторе RAR
Рис. 7.24. Открытие файла index_ov2640.html в текстовом редакторе Notepad++
Удалим с этой страницы ряд кнопок, а оставшиеся переименуем так, чтобы получился простой интерфейс управления роботом. Разумеется, кнопки мы переименуем только на представлении страницы, а наименования закрепленных за
ними переменных оставим. Подробно расписывать здесь этот процесс смысла
нет, поскольку изучение разметки HTML выходит за рамки нашей книги.
Робот телеприсутствия
193
Рис. 7.25. Страница сайта камеры, открытая локально в браузере
Ф а й ло в ы й а рх ив
Напомним, что полные версии листингов и файлов, относящихся к управлению камерой модуля ESP32-CAM, вы найдете в папке CameraRobot сопровождающего книгу
файлового архива (см. приложение).
Сама страница в браузере стала весьма компактной (рис. 7.25). Бегунок Power
управляет скоростью колес, Turning — разностью скорости на левое и правое колеса (поворот), Forward_back — подает на колеса мощность, кнопка
Start Stream начинает трансляцию видео, кнопки Get Still и Enroll Face не задействованы.
Особенности настройки трансляции при подключении
по сети Интернет
Для трансляции сайта при подключении по сети Интернет требуется пробросить через интернет-роутер два порта для IP-адреса, задействованного на
ESP32-CAM, — это порты 80 и 81. Но может случиться, что сайт камеры открывается, а трансляция не идет. Такое произойдет, если после проброса портов
через интернет-роутер основная страница изменит свой порт на настроенный
в роутере, тогда как порт для трансляции видео жестко установлен на странице
в значение 81. В этом случае требуется изменить код в файле index_ov2640.html.
Этот момент требует дополнительного пояснения. Дело в том, что стандартный
порт трансляции сайтов (речь идет о порте IP-протокола, с которым работает
браузер при отображении сайта) имеет номер 80, а трансляция видеопотока
идет через порт 81, который жестко прописан на странице сайта камеры. Когда
194
Глава 7
мы на роутере конфигурируем проброс портов, занимать порт 81 не рекомендуется (порты до 5000 считаются служебными и могут использоваться роутером
для собственных нужд).
Допустим, мы по инструкции из первого раздела главы настроили проброс
портов на 5054 и 5055, а роутер имеет адрес 176.197.141.15 (мы говорим о статическом IP-адресе, выданном нам провайдером). Тогда, чтобы соединиться
с сайтом по Интернету (извне нашей сети), следует набрать в браузере адрес:
176.197.141.15:. Страница сайта откроется, но смотреть видео не получится, — порт не тот! Поэтому нам надо внести в исходный файл страницы правку,
которая проверяет, какой порт был указан после ввода IP-адреса в браузере, и
корректирует значение порта для трансляции видео на самой странице. Логично,
если трансляция будет проходить на порт, который идет следующим по счету за
номером порта сайта, — вне зависимости от того, открыли мы сайт через роутер
или находимся непосредственно в локальной сети сайта камеры.
Итак, в файле index_ov2640.html находим строку:
var streamUrl = baseHost + ':81'
Она задает адрес порта трансляции потокового видео. Заменим указанную строку на следующий код (пример приведен на рис. 7.26 — это язык программирования JavaScript):
var substringArray = baseHost.split(":");
var streamUrl = substringArray[0].concat(":", substringArray[1]);
if (substringArray.length > 2)
{
var port2 = Number(substringArray[2]) + 1;
streamUrl = streamUrl.concat(":", port2);
}
else
{
streamUrl = streamUrl.concat(":", "81");
}
Измененный код, в случае наличия в адресе запроса номера порта, присваивает
переменной streamUrl значение, увеличенное от него на 1, а если в адресе запроса страницы порт не указан, то streamUrl получает значение 81. Например,
если в браузере ввести адрес 192.168.1.203, то трансляция будет ожидаться на
порте 81, а если ввести 176.197.141.15:5054, то трансляция будет ожидаться
на порте 5055.
Робот телеприсутствия
195
Рис. 7.26. Демонстрация изменений в скрипте, позволяющая транслировать видео в Интернет
В приведенном примере: 192.168.1.203 — адрес модуля ESP32-CAM во внутренней сети. Адрес: 176.197.141.15:5054 — адрес ESP32-CAM после проброса портов через роутер в Интернете. При этом 176.197.141.15 — статический
IP-адрес роутера в Интернете, а 5054 и 5055 — порты, настроенные в роутере,
для трансляции сайта модуля 192.168.1.203 через порты 80 и 81 в Интернете.
Настройку роутера для управления роботом через глобальную сеть Интернет
следует произвести по инструкции, данной в разд. «Управляем роботом через
Интернет» этой главы.
Измененную страницу сайта нужно снова сжать и поместить в массив index_
ov2640_html_gz файла camera_index.h программы управления, а старое содержание массива удалить. Для сжатия файла массива можно использовать онлайнконвертер по адресу: https://online-converting.ru (рис. 7.27).
Глава 7
196
Рис. 7.27. Сжатие файла index_ov2640.html онлайн-конвертером
Рис. 7.28. Окно свойств файла index_ov2640.html — его размер: 3672 байта
Робот телеприсутствия
197
Теперь следует передать измененную главную страницу сайта управления в программу. Для этого, получив новый сжатый файл массива index_ov2640_html_
gz, нужно предварительно определить его точный размер в байтах (его можно
посмотреть в свойствах файла, рис. 7.29) — он потребуется при указании размера обновленного массива index_ov2640_html_gz во второй строке файла
camera_index.h:
#define index_ov2640_html_gz_len 3672
А затем для создания массива на основе этого файла следует:
1. Открыть запакованный файл при помощи двоичного редактора HxD.
2. Выделить и скопировать в буфер обмена содержимое открытого файла в
окне двоичного редактора HxD.
3. Вставить содержимое буфера обмена в текстовый редактор (Notepad++ или
Блокнот).
4. Заменить пробелы на символы «, 0х» (запятая, пробел, ноль, икс) и добавить «0x» (ноль, икс) перед первым шестнадцатеричным числом в файле.
Рис. 7.29. Мониторинг скорости потокового видео
Глава 7
198
5. Выделить и скопировать через буфер обмена содержимое полученного в текстовом редакторе файла в массив index_ov2640_html_gz файла
camera_index.h программы, а также скорректировать размер массива на размер файла (в байтах), определенный ранее.
Ус и ле н ие с и г на ла ( ул у ч ше н ие св я з и )
Может оказаться, что обновление видеопотока происходит слишком медленно
(рис. 7.29). Как правило, подобную ситуацию легко исправить, если установить на
ESP32-CAM внешнюю антенну — для этой цели на модуле имеется специальное
гнездо.
Подключение модуля ESP32-CAM к роботу
Для связи модуля ESP32-CAM камеры и модуля ESP32 робота мы воспользуемся последовательным каналом связи и свяжем модули между собой по Serialинтерфейсу, что весьма несложно сделать. Если для камеры ESP32-CAM и ESP32
робота используется один источник питания, для соединения контроллеров
понадобятся всего лишь два проводника. Но вначале рассмотрим изменения в
программе для ESP32-CAM.
Добавление передачи команд по Serial-интерфейсу
в модуль ESP32-CAM
Чтобы подключить ESP32-CAM к роботу, нам потребуется внести изменения в
программный код, а именно в модуль app_httpd.cpp, осуществляющий обработку информации, поступающей от пользователей сайта.
На рис. 7.30 показаны строки 487, 488 и 489, в которых изменена реакция программы на получение сообщений при изменении положения трех ползунков (см.
рис. 7.25). Как мы и отмечали ранее, несмотря на то, что представление ползунков
на сайте было переименовано, имена переменных, которые формируются и передаются со страницы сайта на сервер (сервер — это программа на нашем модуле
ESP32-CAM), не изменились. В строках 487, 488 и 489 производится обработка трех переменных: quality, contrast и brightness (это не переменные С++,
а имена переменных протокола обмена страницы сайта и сервера), в то время
как названия на странице сайта для каждой из этих переменных: Power, Turning
и Forward_back. Когда мы на сайте (в браузере) двигаем указанные ползунки,
переменные со значениями quality, contrast и brightness передаются на сервер, а затем могут быть обработаны программой сервера ESP32-CAM.
Для обработки этих переменных следует создать протокол команд и использовать передачу трех байтов данных по Serial-интерфейсу. Использование более
Робот телеприсутствия
199
Рис. 7.30. Фрагмент программного кода проекта (файла app_httpd.cpp)
сложного, чем ранее, протокола обусловлено тем, что сервер (программа ESP32CAM) передает по Serial-интерфейсу различную служебную информацию
(см. рис. 7.30), которую робот должен суметь отличить от команд движения.
Команда начинается с буквы С, сигнализирующей, что следует ожидать ввод
команды. Далее идет одна из букв: P, T или F. Здесь: P — изменение скорости,
T — управление поворотом, F — инициирует движение вперед или назад. В третьем символе команды закодировано значение, полученное от страницы сайта.
Варианты значений показаны на рис. 7.25 рядом с бегунками параметров.
Соединение модулей ESP32-CAM камеры и ESP32 робота
У нашего робота еще остались свободные полноценные GPIO: GPIO 19 и GPIO
14 — задействуем их в качестве второго Serial-порта (рис. 7.31). Контакт U0R на
модуле ESP32-CAM соединим с GPIO 19 модуля ESP32, а контакт U0T на ESP32CAM — с GPIO 14 ESP32. Остается в программе робота создать второй Serialпорт, назначить GPIO 19 — как TX, GPIO14 — как RX и проверять, что приходит
на этот порт. Если пришла команда на движение, соответствующая протоколу,
следует выполнить ее, задействуя двигательные функции робота.
Глава 7
200
Рис. 7.31. Соединяем модуль ESP32-CAM камеры и контроллер ESP32 робота
Модуль ESP32-CAM должен получать питание от источника 5 вольт — подобный уже используется на роботе, он справится и с дополнительной нагрузкой.
Изменяем программу робота
Возьмем за основу программу из первого раздела этой главы, где робот управлялся от смартфона, установленного на робота в качестве камеры, и внесем в
нее необходимые изменения. Изменения будут произведены в основном файле
программы listing_7_2.ino (листинг 7.2.1) и в файле обработки внешних команд
move_case.h, в остальных модулях изменений нет.
Поскольку получать команды управления робот будет не по Bluetooth, соответствующий порт не открывается, но создается еще один Serial-порт на GPIO 14 и
GPIO 19.
В функции loop() на вновь созданном порту Serial_1 проверяется наличие
команды по шаблону. Если команда пришла полностью (все 3 символа), вызывается функция обработки команд move_case(bt_input, COMMAND_ser), в которую передаются параметры принятой команды.
Робот телеприсутствия
201
Листинг 7.2.1. Файл listing_7_2.ino
// Подключаем библиотеку для создания дополнительных Serial-портов
#include <HardwareSerial.h>
// Создаем объект порт Serial1
HardwareSerial Serial_1(1);
// Подключаем внутренние программные файлы.
#include "motorstep.h"
// Описание моторов
#include "irq_robot.h"
// Работа с прерываниями
#include "move_case.h"
// Реагирование на команды
//=====================================
//+++++++++++++++++++++++++++++++++++++
//=====================================
void setup() {
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы.
motor_off();
// Инициализируем генерацию шагов моторов
timer_setup();
// Открываем Serial-порт
Serial.begin(115200);
// Открываем Serial_1-порт
Serial_1.begin(115200, SERIAL_8N1, 14, 19);//RX-14 , TX -19
Serial.println("Do");
}
//++++++++++++++++++++++++++++++++++++++
int32_t statisticO;
int i = 0;
void loop()
{
// Проверяем наличие команд от смартфона
if (Serial_1.available())
{
// Читаем из буфера очередной символ
char bt_input = (char)Serial_1.read();
Глава 7
202
if (com_calc == 0) if (bt_input == 'C') {
com_calc++;
return;
} else return;
if (com_calc == 1)
if (bt_input == 'F' || bt_input == 'T' || bt_input == 'P')
{
com_calc++;
COMMAND_ser = bt_input;
return;
} else {
com_calc = 0;
return;
}
if (com_calc == 2) {
com_calc = 0;
}
// Обрабатываем принятую команду
move_case(bt_input, COMMAND_ser);
}
else // Если нет новых команд, остановиться, отключить мотор
{
if (flagTimeStop < millis())
{
char bt_input = 0;
COMMAND_ser = 'F';
move_case(bt_input, COMMAND_ser);
}
if (flagTimeOff < millis()) motor_off();
}
}
Функция void move_case(char &bt_input, char &COMMAND_ser) служит для
обработки принятой команды: P — изменяется максимальная скорость, T — начинается поворот, F — движение вперед/назад (листинг 7.2.2). Каждая команда имеет числовое значение, которое обрабатывается. В результате изменяется
максимальная скорость, начинается/заканчивается поворот или начинается/
заканчивается движение вперед/назад.
Робот телеприсутствия
Листинг 7.2.2. Файл move_case.h
unsigned long flagTimeOff;
unsigned long flagTimeStop;
char COMMAND_ser;
int com_calc = 0;
//==============================================
void move_case(char &bt_input, char &COMMAND_ser)
{
if (COMMAND_ser == 'P')
{
uint8_t Kp = uint8_t(bt_input);
maxSPEED = minSPEED + (minSPEED / 255) * 7 * int32_t(Kp);
maxSPEED = constrain(maxSPEED, -8 * minSPEED, 8 * minSPEED);
//
motor_on();
}
if (COMMAND_ser == 'T')
{
int8_t Kt = int8_t(bt_input);
Kt = constrain(Kt, -2, 2);
speed_left = int32_t(Kt) * maxSPEED / 2;
speed_right = -int32_t(Kt) * maxSPEED / 2;
if (Kt != 0)
{
flagTimeOff = millis() + 10000;
flagTimeStop = millis() + 3000;
motor_on();
}
}
if (COMMAND_ser == 'F')
{
int8_t Kf = int8_t(bt_input);
Kf = constrain(Kf, -2, 2);
speed_left = int32_t(Kf) * maxSPEED / 2;
speed_right = int32_t(Kf) * maxSPEED / 2;
if (Kf != 0)
{
flagTimeOff = millis() + 10000;
203
Глава 7
204
flagTimeStop = millis() + 3000;
motor_on();
}
}
// Если требуемая скорость не равна с текущей.
if (speed_last_L != speed_left)
{
LEFT_STEPS_counter
= 2;
}
// Если требуемая скорость не равна с текущей.
if (speed_last_R != speed_right)
{
RIGHT_STEPS_counter
= 2;
}
}
Все готово для запуска робота. Теперь вы сможете удаленно, без прямой видимости, управлять своим роботом — например, следить за тем, что происходит в
доме в ваше отсутствие. Или можете собрать несколько таких роботов и устроить онлайн-битву или онлайн-соревнование по проходу траектории на скорость.
Одним словом, у вас теперь есть свой Аватар!
Ес л и у в ас не с кол ь ко р о б ото в те ле п рис у тс тв и я
Если планируется использовать несколько роботов и управлять ими через Интернет,
для каждого робота следует задать отдельный проброс пары портов: для сайта и для
трансляции.
А мы приступаем к следующему проекту, в котором научим робота балансировать на двух колесах.
ГЛАВА 8
БАЛАНСИРУЮЩИЕ РОБОТЫ
Каждый начинающий конструктор должен уметь сделать балансирующего робота. Такой робот получает информацию о своем наклоне от гиродатчиков и
подкручивает колеса в сторону наклона (балансирует), тем самым уменьшая наклон и восстанавливая равновесие.
Наличие информации об отклонении от вертикали и механизма балансировки
дает роботу существенные плюсы к двигательным возможностям: робот может ориентироваться, катится он с горы или в гору, насколько наклонен вбок.
Роботы, имеющие только пару колес, существенно маневреннее своих четырехколесных собратьев и тратят меньше энергии на повороты. Видимо поэтому и
получили широкое распространение сигвеи и гироскутеры на двух колесах.
Квадрокоптеры также летают устойчиво, потому что получают информацию по
своему наклону от гироприборов и имеют возможность на нее правильно реагировать, уменьшая/увеличивая мощность, подаваемую на определенный винт,
пропорционально отклонению от вертикали (рис. 8.1).
Увеличить
мощность
Уменьшить
мощность
Рис. 8.1. Квадрокоптер в режиме рассогласования мощности моторов
Глава 8
206
Применяемые в робототехнике инерционные приборы называют акселерометрами и гироскопами (гироприборами). Они также широко используются в современных самолетах, ракетах, кораблях и подводных лодках.
Благодаря встроенным гироприборам, большинство смартфонов умеет реагировать на наклон: переворачивать картинку или управлять игрой без помощи
экранных кнопок.
Процесс балансировки робота состоит из двух этапов:
• получение данных от гироприборов и ходовых моторов;
• обработка данных с выработкой команд на исполнительные механизмы
(моторы колес).
Начнем мы знакомство с гироприборами с популярного совмещенного гироскопа-акселерометра-термометра MPU-6050.
П ри ме ча н ие
Существуют и более дорогие и современные гироприборы с гораздо большим потенциалом — например, BNO055, но существенных преимуществ конкретно нашему роботу они не дают.
Гироскоп-акселерометр-термометр MPU-6050
На плате MPU-6050 (рис. 8.2) имеются 8 контактов, но для работы достаточно подключить 4 из них: VCC — питание 3,3 В (возьмем от платы ESP32),
Напряжение питания 3,3–5 В
Отрицательный контакт питания («земля»)
Контакт шины I2C (вывод тактового сигнала)
Контакт шины I2C (передача/прием данных)
Служат для подключения к шине I2C
дополнительных устройств (не используются)
Изменяет адресацию (не используется)
Индикатор наличия электропитания
Настраиваемый сигнал о событии — например,
о переполнении буфера (использовать не обязательно)
Рис. 8.2. Микросхема гироскопа-акселерометра MPU-6050, смонтированная на плате
Балансирующие роботы
207
Рис. 8.3. MPU-6050 подключен к ESP32 (питание от внутреннего стабилизатора)
GND — «земля», SDA и SCL — контакты шины I2C: SCL — вывод тактового сигнала, а SDA — передача/прием данных в обоих направлениях (рис. 8.3).
Шина I2C
I2C — последовательный интерфейс, к которому могут быть подключены одновременно несколько устройств, одно из которых является ведущим, а остальные — ведомыми. В нашем случае ведущим будет контроллер ESP32, а гироскопакселерометр MPU-6050 — ведомым. Каждое устройство на шине I2C должно
иметь уникальный адрес. Когда ведущий начинает передачу, он передает по
шине адрес устройства, к которому выполняется обращение. Устройства, подключенные к шине, проверяют этот адрес, и в случае совпадения начинают обмен информацией.
Глава 8
208
О со б е н но с т ь G P I O дл я ш и н ы I 2 C кон т р ол ле ра E S P 3 2
По умолчанию контакты GPIO 21 и 22 используются в качестве выводов SDA и SCL
шины I2C, но это не является обязательным. Номера GPIO можно настроить в программе, задав их в качестве параметров при инициализации I2C. Это актуально, так
как не на всех макетных платах 21-й контакт GPIO выведен разъемом на макетную
плату и доступен для использования.
Подключение нескольких устройств с одинаковым адресом
к шине I 2 C
Если у вас имеется потребность в нескольких одинаковых устройствах, то нужно позаботиться о том, чтобы они имели разные адреса. Нередко адресацию можно изменить
перемычками на плате устройства, а если подобной функции нет, потребуется специальное устройство I2C-мультиплексор (TCA9548).
Электронный гироскоп
Гироскоп — это прибор, способный реагировать на изменение ориентации объекта. Простейшим примером гироскопа является волчок. Раскрученный волчок
не падает, сохраняя свое вертикальное положение, даже если наклонять поверхность, на которой он установлен. Свойство вращающихся тел удерживать
положение оси вращения в пространстве лежит в основе работы механических
гироскопов.
Но электронный гироскоп серьезно отличается от механического — он не показывает угол отклонения от заданного направления. Электронный гироскоп регистрирует и возвращает текущую угловую скорость (скорость вращения прибора по осям). Зная мгновенную угловую скорость, можно вычислить угол, на
который повернется объект за некоторый промежуток времени:
Ang2 = Ang1 + Δt × Vang ,
(8.1)
где: Ang1 — начальный угол, рассчитанный ранее; Δt — малый промежуток времени, на котором можно считать, что угловая скорость не менялась; Vang — полученное от электронного гироскопа в течение Δt значение угловой скорости;
Ang2 — рассчитанный угол, на который повернулся объект вокруг исследуемой
оси с начала поворота. На рис. 8.4 вращение происходит вокруг оси Z. Показания гироскопа именно по этой оси следует использовать в расчетах.
Погрешности электронного г и р о с копа
Вследствие того, что анализ производится дискретно через заданные промежутки времени, использование только электронного гироскопа может приводить к накоплению
погрешности. Электронные гироскопы также могут иметь ограниченный диапазон
чувствительности и, если угловая скорость выйдет за пределы измерительной шкалы,
измерения станут ошибочными.
Балансирующие роботы
209
Δt×Vang
Ang2
Ang1
Рис. 8.4. Расчет угла поворота по показаниям электронного гироскопа
Электронный акселерометр
Акселерометр служит для измерения ускорения движущегося тела. На основе
его показаний, используя простые формулы, можно отслеживать передвижение
тела в пространстве. Следует учитывать, что простой и дешевый MPU-6050,
в отличие от дорогих сложных устройств, не умеет отделять ускорение свободного падения от ускорения, полученного объектом в результате разгона/торможения. Кроме того, данные от MPU-6050 имеют большой уровень шума, что не
позволяет их использовать напрямую без дополнительной обработки. Однако
в качестве прибора для нахождения углов отклонения объекта от горизонтали
MPU-6050 использовать можно. Для точного измерения объект должен быть
неподвижен или двигаться прямолинейно без ускорения. В этом случае на него
действует только сила притяжения, направленная к центру Земли.
На рис. 8.5 изображен неподвижный объект (робот) на наклонной плоскости.
Координатная ось Y акселерометра робота направлена вперед, ось Z — вниз под
прямым углом к наклонной плоскости, ось X — к наблюдателю. Искомый угол
отклонения объекта от вертикали AngYZ находится по формуле:
AngYZ = arctg
(8.2)
где — проекция ускорения свободного падения на ось Y акселерометра,
проекция на ось Z.
—
Глава 8
210
Можно также использовать формулы:
AngYZ = arcsin
или
AngYZ = arccos
где g— ускорение свободного падения, но для электронного прибора, который
будет рассмотрен далее, рекомендуется использовать формулу 8.2, оперирующую только относительными величинами, — в этом случае нам не придется дополнительно калибровать прибор.
Проекция g
на ось Y
Проекция g
на ось Z
Ускорение
свободного
падения g
AngYZ
Рис. 8.5. Нахождение угла отклонения от вертикали (робот неподвижен)
Балансирующие роботы
211
Шкала значений MPU-6050
Если показания акселерометра в относительных единицах нас вполне устраивают (см. формулу 8.2), то с гироскопом немного сложнее — требуется конвертировать величины, возвращенные прибором, в угловую скорость градусы/секунда или радианы/секунда. Чтобы перевести показания прибора из относительных единиц в привычные абсолютные, следует обратиться к документации. На
сайте производителя InvenSense (www.invensense.com) представлена таблица
шкалы измерений MPU-6050 (табл. 8.1). В частности, чтобы перевести показания гироскопа в градус/сек, их требуется поделить на 131 (при максимальной
чувствительности прибора). Для перевода ускорения в привычную для нас систему мер требуется поделить показания прибора на 16384, а затем умножить
на 9,8 м/с2.
Таблица 8.1. Характеристики шкалы измерений MPU-6050
Получение и обработка данных от MPU-6050
Контроллер MPU-6050 посылает запрос на получение данных, гироприбор возвращает запрошенные значения. В листинге 8.1.1 приведен пример небольшой
библиотеки, в которой реализованы функции инициализации прибора, получение данных от него и расчет компенсации смещения нуля гироскопа.
Листинг 8.1.1. Получение данных от MPU-6050 (файл gyro_acsel.h)
// Библиотека для работы с протоколом I2C (порты A5/SCL и A4/SDA)
#include <Wire.h>
#define ALFA 0.01
#define ONE_ALFA 0.99//
Глава 8
212
// Упрощенный I2C адрес нашего гироскопа/акселерометра MPU-6050.
const int MPU_addr = 0x68;
// Переменные для хранения данных, возвращаемых прибором.
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
double CompensatorZ, CompensatorX, CompensatorY;
unsigned long timer = 0;
//////////////////////////////////////////////////////////////////////
///
/// Запуск гироскопа
//////////////////////////////////////////////////////////////////////
///
void giroscop_setup()
{
/* Enable I2C */
Wire.begin();//22,23); //SDA //SCL
#ifdef ESP8266
Wire.setClockStretchLimit(1000); // Allow for 1000us of clock
stretching
#endif
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
MPU-6050
Wire.write(0);
// Производим запись в регистр энергосбережения
// Устанавливаем его в ноль
Wire.endTransmission(true);
CompensatorZ = 0;
CompensatorX = 0;
CompensatorY = 0;
}
//////////////////////////////////////////////////////////////////////
///
//// Считывание данных с mpu6050
//////////////////////////////////////////////////////////////////////
///
void Data_mpu6050()
{
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // Готовим для чтения регистры с адреса 0x3B.
Wire.endTransmission(false);
// Запрос 14 регистров.
Wire.requestFrom(MPU_addr, 14, true);
Балансирующие роботы
213
// 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();
}
//////////////////////////////////////////////////////////////////////
///
// Компенсация нуля гироскопа
void Calc_CompensatorZ(float mill_sec)
{
long ms = mill_sec;
float i = 0;
CompensatorZ = 0;
CompensatorX = 0;
CompensatorY = 0;
timer = millis();
unsigned long endtime = millis() + ms;
while (endtime > timer) {
timer = millis();
Data_mpu6050();
CompensatorZ += (float)(GyZ);
CompensatorX += (float)(GyX);
CompensatorY += (float)(GyY);
delay(2);
i++;
}
CompensatorZ /= i;
CompensatorX /= i;
CompensatorY /= i;
}
Глава 8
214
Итак, установим гироскоп-акселерометр MPU-6050 на робота, как показано на
рис. 8.6, проведем тесты по получению от него информации и рассчитаем углы
наклона робота. Воспользуемся для этого формулами 8.1 и 8.2, а также формулой комплементарного фильтра (8.3), который производит пропорциональное
смешивание данных от гироскопа и акселерометра, что позволит избавиться от
шума (колебания) данных акселерометра и устранить погрешность, которая постоянно накаливается при расчете угла только от гироскопа:
Akn = (Akn-1 + S×dt)×Kf + Aa×(1–Kf),
(8.3)
где: Akn — рассчитанный угол, Akn-1 — угол, рассчитанный в прошлом опросе;
S — угловая скорость, полученная от гироскопа; dt — время, прошедшее с момента прошлого опроса гироприбора и расчета Akn-1; Kf — коэффициент влияния на результат расчета, основанного только на угловой скорости; Aa — текущий угол, рассчитанный по показаниям акселерометра; 1–Kf — коэффициент
влияния на результат угла по показаниям акселерометра.
MPU-6050
Рис. 8.6. Робот с установленным MPU-6050
Балансирующие роботы
215
Коэффициент влияния Kf для MPU-6050 изменяется от 0,96 до 0,99: при уменьшении становятся заметны колебания от акселерометра, при увеличении может
накапливаться погрешность, связанная с гироскопом. По факту берутся 0,99 частей от расчета по гироскопу и смешиваются с 0,01 частью расчета по акселерометру.
Для MPU-6050, установленного, как показано на рис. 8.6, ось вращения колес
соответствует оси Y прибора, поэтому в расчете угла наклона по показаниям
акселерометра будут использоваться показания акселерометра по осям X и Z и
данные угловой скорости по оси Y.
Программа опроса MPU-6050 состоит из двух частей: модуля получения данных от MPU-6050 (файл gyro_acsel.h, приведенный ранее в листинге 8.1.1)
и основной программы опроса MPU, приведенной в листинге 8.1.2 (файл
listing_8_1.ino).
Файл gyro_acsel.h (см. листинг 8.1.1) содержит описание переменных, используемых для хранения полученных от MPU-6050 значений: AcX, AcY, AcZ, Tmp,
GyX, GyY и GyZ. Коэффициенты из формулы 8.3: Kf — это константа ONE_ALFA,
а (1–Kf) — константа ALFA. Константа MPU_addr содержит адрес MPU-6050 на
шине I2C (0x68).
Функция giroscop_setup() запускается один раз в начале программы, инициализирует протокол I2C (Wire) и включает гироприбор.
Функция Data_mpu6050() производит запрос данных от MPU-6050 и помещает
полученные данные в 16-разрядные переменные: AcX, AcY и AcZ — показания
акселерометра по трем осям, Tmp, GyX, GyY и GyZ — показания угловой скорости
вращения вокруг соответствующих осей, Tmp — температура (может использоваться в расчете температурного смещения показаний прибора).
Функция Calc_CompensatorZ(float mill_sec) останавливает работу программы на заданное количество миллисекунд, в течение которых робот должен быть
неподвижен, что позволяет рассчитать смещение нуля гироскопа.
Итак, робот неподвижен, с MPU-6050 снимаются показания угловой скорости:
показания не нулевые и колеблются вокруг некоторого значения, — по показаниям прибора присутствует угловая скорость, но реального вращения нет.
Получаемые показания суммируем, а затем делим на их количество, и получаем
среднее значение смещения нулевой точки для каждой оси. Смещения заносятся
в переменные: CompensatorZ, CompensatorX и CompensatorY и в дальнейшем
для повышения точности должны вычитаться из показаний угловой скорости,
полученной от гироскопа.
Глава 8
216
Вторая часть программы (файл listing_8_1.ino) служит для расчета угла наклона
робота вокруг оси Y гироприбора. Переменные: dt, t0, t2 и t_period входят в
расчет длительности периода между получением данных от MPU. Переменные:
AcYsum, GyXsum0, OldGyro, OldAcYsum, AcYsumOld, Acsel и Gyro используются
в расчете угла наклона робота.
Переменная _1_d_131 — коэффициент перевода угловой скорости, полученной
от гироприбора, в величину градусы/секунды.
Повторение расчетов происходит в функции loop через время, не менее чем
t_period — 5000 микросекунд.
Значение Dt = double(dt) * 0.000001 — это точное время предыдущего периода в секундах, dt — это микросекунды.
Показания акселерометра рассчитываются в формуле:
// с учетом поворота прибора
double Atan = atan2(AcX, AcZ);
if(Atan>-PI/2.0)
Acsel = (Atan-PI/2.0) * RAD_TO_DEG;
else Acsel = (PI*1.5+Atan) * RAD_TO_DEG;
где: Atan — угол, рассчитанный по показаниям акселерометра, RAD_TO_DEG —
коэффициент перевода радиан в градусы.
Нулевые показания прибора должны быть получены при горизонтальном положении платы MPU-6050 робота. Для работы в вертикальном положении (балансирующий робот) показания нужно сместить на 90 градусов. Для этого служит эта процедура, позволяющая получать правильные (смещенные) значения
при всех углах Atan.
Угловая скорость рассчитывается по формуле:
Gyro = - (float(GyY) - CompensatorY)
* _1_d_131;
Фактически угловая скорость уменьшается на дрейф нуля (CompensatorY) и
конвертируется в градусы/секунды.
Показания акселерометра и гироскопа прибора MPU-6050 имеют особенность,
которую необходимо учесть при расчетах. При росте угла по акселерометру
гироскоп показывает отрицательную угловую скорость. Но, как известно, при
росте угла угловая скорость должна быть положительной, при уменьшении
угла — отрицательной. Поэтому для получения правильного результата следует согласовать показания гироскопа и акселерометра, инвертировав показания
либо угловой скорости, либо угла наклона.
Балансирующие роботы
217
Расчет значения угла наклона по правилам комплементарного фильтра выполняется по следующей формуле:
AcYsum = ONE_ALFA * (OldAcYsum + Gyro * Dt) + ALFA * Acsel.
По правилам расчета комплементарного фильтра:
ONE_ALFA + ALFA = 1, ONE_ALFA = 0.99, ALFA = 0.01.
Расчет смещения угла при использовании только гироскопа:
GyXsum0 =GyXsum0+Gyro * Dt;
Гироскоп ничего не знает о начальном значении угла наклона робота и считает
только смещение.
Чтобы убедиться в правильности выбора для расчетов именно комплементарного фильтра, визуально проследим за «особенностью» работы акселерометра,
для чего воспользуемся плоттером по последовательному соединению.
П лот те р по по с ле д о в а те л ь ном у со е д и не н и ю
Плоттер можно запустить из среды Arduino IDE командой меню Инструменты |
Плоттер по последовательному соединению. Плоттер рисует на экране графики
по числовым данным, полученным по Serial-каналу от контроллера. Чтобы вывести
несколько графиков, данные должны передаваться через пробел. Отрисовка следующей позиции графика происходит после получения кода переноса строки — команды
контроллера Serial.println().
Перенаправим вывод рассчитанных данных: результирующий угол (комплементарный фильтр), отклонение угла (гироскоп), результирующий угол (акселерометр). Для этого в конце программы производится вывод рассчитанных
значений в последовательный порт. Вывод организован в цикле. Это сделано для
прорисовки графика в плоттере по последовательному соединению. Если требуется вывести результаты в порт для просмотра, требуется убрать комментарий
со строки: // if (i > 5). Можно также заменить значение 5 на большее.
Листинг 8.1.2. Основная программа опроса MPU (файл listing_8_1.ino)
#include "gyro_acsel.h"
// Переменные управления интервалами опроса гироприбора
// и интервалами управления
unsigned long dt;
unsigned long t0;
unsigned long t2;
double AcYsum, GyXsum0;
Глава 8
218
double OldGyro = 0;
double OldAcYsum = 0;
double AcYsumOld = 0;
int32_t t_period;
//double
double
_1_d_131 = 1.0 * DEG_TO_RAD / 131.0;
_1_d_131 = 1.0
/ 131.0;
double Acsel = 0;
double Gyro = 0;
void setup() {
Serial.begin(115200);
Serial.println();
giroscop_setup();
delay(100);
Calc_CompensatorZ(2000);
t0 = micros() - 5000;
t2 = micros();
}
void loop()
{
t_period = 5000;
static int i = 0; // для торможения вывода
uint32_t micros_;
micros_ = micros();
// Если период < t2 (прошло 5000, если да, производим очередной
// расчет)
if (micros_ < t2) return;
dt = micros() - t0; // Длительность предыдущего периода
// регулирования.
t0 += dt; // Точка начала нового периода регулирования.
// Переводим в секунды время от предыдущего опроса.
double Dt = double(dt) * 0.000001;
// Опрос гироприбора:
Data_mpu6050();
// Расчет угла по показаниям акселерометра с учетом поворота прибора
double Atan = atan2(AcX, AcZ);
if(Atan>-PI/2.0)
Балансирующие роботы
219
Acsel = (Atan-PI/2.0) * RAD_TO_DEG;
else Acsel = (PI*3.0/2.0+Atan) * RAD_TO_DEG;
// Скорость угловая - падения
Gyro = - (float(GyY) - CompensatorY)
* _1_d_131;
// Комплементарный фильтр
AcYsum = ONE_ALFA * (OldAcYsum + Gyro * Dt) + ALFA * Acsel;
GyXsum0 =GyXsum0+Gyro * Dt;
OldAcYsum = AcYsum;
t2 = t0 + t_period;
if (i > 100)
{
i = 0;
Serial.print(GyXsum0);
Serial.print(" ");
Serial.print(Acsel);
Serial.print(" ");
Serial.println(AcYsum);
}
i++;
}
Загрузите программу в контроллер робота и после начала ее работы откройте
плоттер. Плавно наклоняйте робота вдоль оси колес — в плоттере должны отрисоваться три кривые (рис. 8.7). Верхняя синусоида — это результат расчета с использованием только гироскопа (смещение кривой вдоль оси Y графика вверх
на 90 градусов — произвольное). Две оставшиеся кривые совпадают по частоте
и уровню, но одна из них плавная — расчет угла по комплементарному фильтру,
а вторая имеет значительные резкие всплески — это расчет угла по показаниям
акселерометра.
Колебание показаний акселерометра приводит к тому, что в динамических системах использование его без дополнительной математической обработки затруднительно, но смешивание данных от гироскопа и акселерометра дает четкую картину по отклонениям от вертикали.
Ре ше н ие п р о бле м ы о бла м ыв а н и я ра з ъ е ма
Чтобы не повредить гнездо microUSB контроллера ESP32, придерживайте соединительный кабель так, чтобы при поворотах робота на это гнездо не оказывалось механическое воздействие.
220
Глава 8
Рис. 8.7. Расчет изменения угла по гироскопу (верхняя синусоида), акселерометру (нижняя
синусоида со всплесками), при помощи комплементарного фильтра (нижняя плавная синусоида)
Сопоставим положение робота и результат на экране. Если при установке и удержании робота в вертикальном положении кривая комплементарного фильтра
близка к нулевой точке по оси Y графика, значит, оси выбраны правильно.
В противном случае это может означать, что прибор на вашей плате расположен
по-другому, и следует проверить и, если надо, изменить ось, используемую в качестве базовой для расчета.
Следует заметить, что возможен также температурный дрейф показаний гироприбора — тогда погрешность будет зависеть и от температуры прибора. Учесть
температурную погрешность также можно, но эта задача выходит за рамки нашей книги.
Основы регулирования: ПИД-регулятор
Прежде чем приступить к разработке программы балансировки, рассмотрим
основы регулирования и разберемся с ними на более простых примерах.
Для регулирования анализируется состояние системы и вырабатывается управляющее воздействие. Применяются три типа обработки состояния системы при
расчете величины управляющего воздействия: пропорциональный, интегральный, дифференциальный. Не редко эти типы регулирования применяют совместно, отсюда возникла аббревиатура ПИД-регулятор.
• Пропорциональное регулирование применяется тогда, когда управляющее воздействие пропорционально ошибке регулирования.
Балансирующие роботы
221
• Дифференциальное регулирование применяется тогда, когда управляющее воздействие пропорционально скорости изменения ошибки регулирования.
• Интегральное регулирование применяется тогда, когда управляющее воздействие пропорционально интегралу ошибки регулирования по времени.
Рассмотрим работу ПИД-регулятора на примере электрического мотора. Пусть
в нашем распоряжении имеется электрический мотор постоянного тока, подключенный к драйверу. Драйвер управляется от контроллера при помощи
ШИМ — датчик снимает с вала мотора скорость, которая поступает в контроллер. Требуется научить контроллер управлять скоростью мотора.
Ч то та ко е Ш И М ?
ШИМ — это широтно-импульсная модуляция, в англоязычной интерпретации: PWM
(Pulse-Width Modulation). Управляющий контроллер генерирует импульсы с определенной частотой, которые подаются на вход управления мощностью управляемого
устройства. Управляющий контроллер может изменять ширину импульсов от 0 до
100% заполнения, при этом выходная мощность устройства изменяется от 0 до 100%
линейно (в главе 1 мы уже встречались с ШИМ).
В качестве информации о текущем состоянии регулируемой системы выступает
скорость на валу мотора, а регулировать состояние системы мы будем, изменяя
мощность, подаваемую на мотор при помощи изменения ширины управляющих
импульсов (ШИМ) на драйвер мотора.
Рассмотрим действия звеньев ПИД-регулятора подробнее.
• Пропорциональное регулирование (П-регулятор) предполагает, что управляющее воздействие (у нас — величина ШИМ) прямо пропорционально
разности между требуемым значением выходной величины (требуемая скорость) и реально измеренным значением (измеренная скорость).
Имея заданную скорость Speedcom, с которой требуется вращать вал мотора,
контроллер получает от датчика текущую измеренную скорость Speedsens—
она может отличаться от заданной скорости. Найдем разницу между заданной и реальной скоростью Errorspeed:
Errorspeed = Speedcom – Speedsens.
Мощность Emot, подаваемая на мотор, рассчитанная с использованием только пропорционального регулятора:
Emot = Errorspeed × Kp,
где Kp — коэффициент пропорциональности.
Глава 8
222
В результате применения пропорционального звена регулирования скорости мы получим колебательную систему (рис. 8.8) — реальная скорость будет колебаться вокруг заданного значения. Это связано с тем, что направление мощности прямо пропорционально отклонению, что в реальных системах, которые обладают инерционностью, приведет к перерегулированию и
колебаниям.
Недостатком этого звена является отсутствие реакции на изменение нагрузки. Например, если мы настраивали коэффициент пропорциональности без
нагрузки, а затем начнем тормозить вал мотора, пропорциональное звено
никак не сможет это компенсировать. Если же, наоборот, коэффициент пропорциональности будет настроен при нагрузке на валу мотора, а затем эту
нагрузку убрать, скорость вала двигателя начнет значительно колебаться и
регулирования не получится (рис. 8.9).
Уменьшить колебания можно, добавив в систему регулирования анализ
скорости изменения ошибки — дифференциальное звено.
3,50
3,00
2,50
2,00
Реальная
скорость
1,50
Заданная
скорость
1,00
0,50
0,00
0
2
4
6
8
10
12
14
16
Рис. 8.8. Пропорциональное регулирование (П-регулятор): незатухающие колебания
6,00
5,00
Реальная
скорость
4,00
3,00
Заданная
скорость
2,00
1,00
0,00
0
2
4
6
8
10
12
14
16
Рис. 8.9. Пропорциональное регулирование (П-регулятор): самовозбуждение системы
из-за высокого коэффициента П-регулятора
Балансирующие роботы
223
• Дифференциальное регулирование (ПД-регулятор) предполагает, что управляющее воздействие прямо пропорционально разности текущей ошибки
регулирования и ошибки регулирования прошлого измерения, поделенного
на время между проведением данных измерений. Фактически это скорость
изменения ошибки регулирования.
Если ошибка регулирования с прошлого измерения уменьшилась, то
скорость будет отрицательной, в противном случае — положительной.
Отрицательная скорость (при положительном знаке ошибки) говорит о
том, что ошибка регулирования уменьшается — система приходит в норму.
Наличие подобного звена уменьшит влияние пропорционального звена и
снизит перерегулирование.
Положительная скорость (при положительном знаке ошибки) говорит о
том, что ошибка растет, — в этом случае дифференциальное звено увеличит
управляющее воздействие и «поможет» пропорциональному звену регулирования.
Для использования дифференциального регулирования требуется, чтобы
измерения производились периодически через небольшие промежутки
времени. Этот параметр регулирования подразумевает использование в
качестве аргументов скорость изменения ошибки регулирования и время
между измерениями-расчетами скорости:
derror =
Errorspeed – ErrorspeedOld
dt
,
Emot = Errorspeed × Kp + derror × Kd.
Пример уменьшения колебаний приведен на рис. 8.10.
6,00
5,00
4,00
Cкорость с П-регулятором
3,00
Cкорость с ПД-регулятором
Заданная скорость
2,00
1,00
0,00
0
5
10
15
Рис. 8.10. Пример уменьшения колебаний при помощи ПД-регулятора
Глава 8
224
В некоторых случаях в расчет принимается скорость изменения скорости,
или ускорение — расчет его подобен. Смысл применения аналогичный —
стабилизация системы регулирования, уменьшение колебаний.
• Интегральное регулирование (ПИД-регулятор) применяется для компенсации недостаточного влияния пропорционального звена. Оно также позволяет существенно уменьшить коэффициент пропорциональности при пропорциональном звене, чем снижает колебания (рис. 8.11). Интегральное
звено необходимо при регулировании в системах с переменной нагрузкой:
Ierror = IerrorOld + Errorspeed × dt,
dEmot = Errorspeed × Kp + derror × Kd + Ierror × Ki,
Emot = EmotOld + dEmot.
6,00
5,00
4,00
Cкорость ПИД-регулятор
3,00
Cкорость ПД-регулятор
Заданная скорость
2,00
1,00
0,00
0
2
4
6
8
10
12
14
16
Рис. 8.11. Пример качественного регулирования (ПИД-регулятор)
Настройка ПИД-регулятора
балансирующего робота
Для настройки регулятора мы загрузим программу listing_8_2.ino (листинг 8.2).
Это адаптированный к балансировке без управления программный модуль, который подойдет для отработки навыков настройки параметров балансировки.
Предварительно проверьте настройки направления вращения моторов (файл
motorstep.h, строки с 8 по 12) и поправьте их, если у вашего робота в предыдущих проектах были с ними различия.
В файле listing_8_2.ino мы будем изменять строки с 46 по 55, где представлены
различные формулы расчета скорости реакции робота. Также изменению могут
быть подвергнуты строки, задающие коэффициенты:
Балансирующие роботы
225
Ka = 1200.0 / ( PI )
Kg = 300.0 / ( PI )
Для начала раскомментируйте формулу под цифрой //1 (удалите перед ней две
косые черты), а остальные оставьте закомментированными:
//1
Speed = AcYsum
* Ka;
Если робот отклонился от вертикали, для возвращения в вертикальное положение ему следует доработать колесами в сторону своего наклона. Это должно
привести к тому, что робот будет устойчиво колебаться в районе своего равновесного положения. Но если скорость, с которой робот отрабатывает угол наклона, станет больше определенного значения (а скорость эта прямо пропорциональна углу наклона), робот может перевалиться в сторону обратную от наклона, начать колебаться с увеличивающейся амплитудой и в результате упадет.
Если попытаться сильно уменьшить коэффициент пропорциональности Ka, то
робот, наоборот, не будет доходить до вертикали и упадет сразу. В любом случае
качественной балансировки не получится. Так что изменяйте Ka и отслеживайте
изменения поведения робота.
Попробуем другой вариант: раскомментируйте формулу под цифрой //2, а
остальные оставьте закомментированными:
//Speed = AcYsum
//2
Speed = Gyro
* Ka;
* Kg;
Здесь мы возьмем угловую скорость (скорость падения робота), подберем коэффициент Kg и посмотрим, как робот будет реагировать на прямо пропорциональное угловой скорости изменение своего положения. В положении покоя
робот, вне зависимости от наклона, будет бездействовать, и начнет сопротивляться изменению угла наклона. Кажется, вот еще немного, и он встанет, но это
не происходит, потому что гироскоп ничего не знает о точке равновесия робота.
Робот падает, но плавно.
Попробуем теперь следующий вариант: раскомментируйте формулу под цифрой //3, а остальные оставьте закомментированными:
//3
Speed = AcYsum
* Ka + Gyro
* Kg;
Мы научились работать с пропорциональным (по углу наклона) и дифференциальным (по производной по времени от угла наклона — угловой скорости)
регуляторами, а теперь подставим их в работу вместе. Если робот, используя
Глава 8
226
такой регулятор, резко уходит в нестабильное состояние, уменьшите коэффициенты по обоим звеньям вдвое, — робот не станет балансировать на месте, но
некоторое время проедет до своего падения. Если не получилось с первого раза,
попробуйте поиграть с коэффициентами сначала одного звена — Ka, а затем
другого — Kg, запомните, как влияет изменение определенного коэффициента,
и стремитесь к коэффициентам, которые улучшают качества балансировки.
Попробуем и еще один вариант: раскомментируйте формулу под цифрой //4,
а остальные оставьте закомментированными:
//4
Speed = AcYsum
* Ka + Gyro
* Kg + XSpeed;
Теперь робот едет и обязательно падает. Дело в том, что мы забыли про ситуацию, когда робот уже имеет некоторую скорость и все равно наклонен. Мы не
учитываем текущую скорость движения робота, но ведь все наклоны робота
происходят в его собственной статической относительно колес системе координат. Так что введем в расчет результирующей скорости скорость, с которой
робот уже движется, — переданную его колесам в прошлом периоде расчета:
XSpeed. Возможно, придется значительно уменьшить коэффициенты Ka и Kg.
Если затрудняетесь с настройкой, верните те коэффициенты, которые были
первоначально:
Ka = 1200.0 / ( PI );
Kg = 300.0 / ( PI );
Фактически мы изменяем не скорость робота, а его ускорение, так как в ситуации наличия у робота ускорения появляется сила инерции, которая направлена
против ускорения, и в общем случае вычисляется по формуле:
Fi = ma,
где Fi — сила инерции, m — масса объекта, a — ускорение объекта.
Теперь робот станет балансировать качественно, но, к сожалению, будет уезжать, и всегда в одну сторону. Это связано с отклонением нуля акселерометра от
равновесного состояния робота.
На рис. 8.12 продемонстрировано наглядно, что происходит с роботом, если его
центр тяжести смещен — например, сбоку есть груз (рис. 8.12, слева). В этом
случае робота постоянно тянет наклониться вперед в попытке компенсировать
наклон, и робот движется в сторону наклона (рис. 8.12, справа). Конечно, реальный робот не так сильно разбалансирован, но даже небольшого отклонения
Балансирующие роботы
Сила
тяжести
Груз
MPU-6050
Груз
U
MP
-60
50
227
Сила
тяжести
Направление
движения
Направление
движения
Рис. 8.12. Робот компенсирует движением
смещение своего центра тяжести
относительно вертикального положения
Рис. 8.13. Робот наклоняется, пытаясь
вывести в ноль наклон MPU-5060
достаточно, чтобы он начал движение, а скорость зависит от величины смещения равновесного положения робота от нулевого значения отклонения гироприбора.
Второй вариант разбалансировки — это неточная установка гироприбора. В нашем роботе MPU-6050 установлен на макетной плате и держится на контактахштырьках, которые могут недостаточно точно фиксировать его положение, —
MPU-6050 наклонен (рис. 8.13, слева), робот, стремясь вывести угол гироприбора в ноль, наклоняется, начинает падать, пытается компенсировать падение
и катится (рис. 8.13, справа).
Если перед отъезжающим роботом окажется подъем, он сможет остановиться, стоять и балансировать, но одновременно управлять балансировкой и скоростью движения робота мы еще не научились.
Тем не менее нами уже создан качественный ПД (пропорционально-дифференциальный) регулятор: пропорциональное звено — по углу наклона, дифференциальное — по скорости наклона.
Листинг 8.2. Подбор коэффициентов балансировочных звеньев
(файл listing_8_2.ino)
#include "defin.h"
#include "gyro_acsel.h"
#include "motorstep.h"
// Описание моторов
#include "irq_robot.h"
// Работа с прерываниями
Глава 8
228
void setup() {
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы.
motor_off();
// Инициализируем генерацию шагов моторов
timer_setup();
Serial.begin(115200);
giroscop_setup();
delay(100);
Calc_CompensatorZ(4000);
t0 = micros() - 5000;
t2 = micros();
Speed = 0;
}
uint32_t micros_;
void loop()
{
int32_t speed_L ;
int32_t speed_R;
t_period = 5000;
static int i = 0; // Для торможения вывода
micros_ = micros();
if (micros_ < t2) return;// Прошло 5000 мкс
// Опрос гироприбора:
Data_mpu6050();
dt = micros() - t0; // Длительность предыдущего периода
// регулирования.
t0 += dt; // Точка начала нового периода регулирования.
double Dt = double(dt) * 0.000001;
// Расчет угла по показаниям акселерометра
Acsel = (atan2(AcX, AcZ)) - PI / 2.0; // * RAD_TO_DEG;
// Скорость угловая (в радианах) - падения
Gyro = - (double(GyY) - CompensatorY)
* _1_d_131;
// Комплементарный фильтр
AcYsum = ONE_ALFA * (AcYsum + Gyro * Dt) + ALFA * Acsel;
t2 = t0 + t_period;
Балансирующие роботы
229
Ka = 1200.0 / ( PI );
Kg = 300.0 / ( PI);
//1
Speed = AcYsum
* Ka;
//2
//Speed = Gyro
* Kg;
//3
//Speed = AcYsum
* Ka + Gyro
* Kg;
* Ka + Gyro
* Kg + XSpeed;
//4
//Speed = AcYsum
Speed *= 400.0;
speed_L = (Speed);
speed_R = (Speed);
speed_L = constrain(speed_L, -maxSPEED, maxSPEED);
speed_R = constrain(speed_R, -maxSPEED, maxSPEED);
//Speed - это число шагов за 100 секунд
SetSpeed(speed_L, speed_R);
// Возвращаем обработанное значение скорости в первоначальную формулу
XSpeed = (speed_L + speed_R) / 800.0;
}
Как остановить робота?
На практике весьма трудно подобрать такое положение гироприбора на роботе, которое точно соответствует центру масс робота. Положение робота, когда
ноль гироприбора достигнут, а центр масс смещен вперед или назад, приводит
к постоянному стремлению робота наклониться в сторону, которая «тяжелее».
Алгоритм балансировки, рассмотренный в предыдущем разделе, пытаясь привести робота в состояние «нуля» гироприбора, а не в состояние равновесия робота (когда вектор силы тяжести, действующей на центр масс робота, проходит
через ось колес), приводит к тому, что робот постоянно катится в «тяжелую»
сторону.
Отсюда вытекает задача: улучшить алгоритм балансировки и научить робота
компенсировать смещение нуля гироприбора для достижения положения, когда
«ноль» гироприбора соответствует состоянию равновесия. Это приведет к тому,
что движение робота прекратится.
Глава 8
230
Груз
Груз
Сила
инерции
Сила
тяжести
Сила
тяжести
Направление
ускорения
Рис. 8.14. Робот, компенсирующий смещение своего центра масс
Напрашивается решение: если робот наклонен (рис. 8.14, справа), придать ему
ускорение в направлении наклона (рис. 8.14, слева), — это приведет к появлению силы инерции, и наклон робота уменьшится.
О бра т и те в н и ма н ие !
Для компенсации наклона нужно создать именно ускорение, а не скорость! При равномерном движении с любой скоростью сила инерции равна нулю.
Итак, усовершенствуем алгоритм балансировки — для компенсации ошибки согласования нуля гироприбора и равновесия робота создадим два дополнительных звена регулятора: интегральное и пропорциональное.
Интегральное звено
Введем накопительное (интегральное звено) скорости движения робота по времени (иначе — пройденной дистанции). Если робот катится в определенную
сторону, считаем, что робот имеет ошибку согласования нуля гироприбора и
своего равновесного положения.
Робот катится, увеличивается пройденный путь, в результате значение соответствующего звена регулятора Move×Km растет и компенсирует ошибку от звена
по углу Ang×Kp:
Sn = Sn-1 + Ang×Kp + Gy×Kg + Move×Km,
(8.4)
где: Sn — расчетная скорость, Sn-1 — текущая скорость (с которой робот уже
движется), Ang — рассчитанный по гироприбору угол наклона робота, Kp —
коэффициент пропорциональности влияния угла наклона на результат, Gy —
угловая скорость (скорость падения робота по гироскопу), Kg — коэффициент
Балансирующие роботы
231
пропорциональности влияния угловой скорости на результат, Move — интеграл
от скорости передвижения по времени (или пройденный роботом путь), Km —
коэффициент пропорциональности влияния пройденного пути на результат.
На практике применение формулы 8.4 приведет к колебаниям робота: он проедет определенное расстояние в одну сторону, остановится и поедет в другую.
Амплитуда колебаний будет зависеть от Km, и может быть весьма большой (до
нескольких метров для нашего робота). Это связано с тем, что пройденный путь
будет расти даже тогда, когда робот уже имеет отрицательное линейное ускорение — то есть тормозит. Путь будет расти до момента полной остановки робота,
что приведет к перерегулированию, которое роботу придется компенсировать
обратным движением.
Пропорциональное звено
Для компенсации колебания от звена Move×Km введем звено по линейной скорости (производной от пути по времени) dMove×Kdm:
Sn = Sn-1 + Ang×Kp + Gy×Kg + Move×Km + Move×Kdm
(8.5)
где: dMove — линейная скорость робота, рассчитанная по шагам моторов, Kdm —
коэффициент пропорциональности влияния dMove на результат.
Работает это следующим образом. Робот получает дополнительную корректировку скорости от звена по линейной скорости. Причем, чем больше скорость,
тем сильнее корректировка скорости (пропорциональное звено). В результате
робот останавливается раньше, чем звено Move переполнится. После остановки
звено по линейной скорости, соответственно, перестает влиять на результат —
ведь линейная скорость отсутствует, а вот накопленное значение Move по дистанции сохранится.
Удобно, если программное обеспечение и моторы робота позволяют задавать
не скорость, а ускорение. В этом случае коэффициенты немного изменятся, но в
целом суть формулы сохранится:
Accel = Ang×Kp + Gy×Kg + Move×Km + Move×Kdm
(8.6)
где: Accel — ускорение, с которым должен двигаться робот до следующего опроса гироприбора и перерасчета ускорения (5–10 миллисекунд).
Алгоритм балансировки (формула 8.5) можно еще немного «усилить», если ввести звенья углового ускорения и ускорение по линейной скорости, но делать это
нужно взвешенно и осторожно, так как данные по ускорениям зашумлены. В программе (листинг 8.3) параметры углового ускорения и линейного ускорения интерполируются (используются средние значения за 10 итераций расчета).
232
Глава 8
Программа, помогающая роботу стоять
Рассмотрим программу, помогающую роботу стоять (листинг 8.3). Функцией
setup() в ней инициализируются шаговые моторы, запускаются прерывания по таймеру для генерации шагов, подключается MPU-6050, запускается
4-секундный цикл пересчета отклонений нуля гироскопа.
В периодически повторяющуюся функцию loop() внесены изменения, которые
будут рассмотрены далее.
Перед расчетом скорости моторов описаны два условных блока, в результате исполнения которых робот может завершить расчет и, не пересчитывая скорость,
уйти на следующую итерацию расчета угла, — это блоки обработки падения:
• if ( flag_crash) {…} — если робот упал… При обработке этого блока производится проверка состояния угла наклона, и если он близок к состоянию
подъема, включаются моторы и снимается флаг падения (flag_crash);
• if (abs(Move) > MoveLimit) {…} — если робот не может сбалансироваться, например, упал, отключаются моторы, обнуляются все расчетные характеристики, робот «отдыхает» 6 секунд, после чего его можно поднять, и он
заново начнет балансировать.
Параметры:
XL = counter_stepL; counter_stepL = 0;
XR = counter_stepR; counter_stepR = 0;
определяют получение перемещения по левому и правому моторам робота в
шагах от начала предыдущего опроса.
Линейная скорость робота (в шагах) при этом рассчитана по формуле:
dMove = double(XL + XR) * 0.5 / Dt;
Линейное ускорение сглаживается за 10 итераций:
ddMove = ddMove * 0.9 + (dMove - dMoveOld) * 0.1 / Dt;
dMoveOld = dMove;
Перемещение рассчитывается как сумма шагов колес, деленная на 2, шаги за
предыдущие периоды складываются (с учетом знака направления вращения):
Move += (XL + XR) / 2.0; // Пройденный путь
Угловое ускорение также рассчитывается за последние 10 итераций:
dGyro = 0.9 * dGyro + 0.1 * ((Gyro - GyroOld) / Dt);
GyroOld = Gyro;
Балансирующие роботы
233
Обновленная скорость рассчитывается по формуле:
Speed = (AcYsum) * Ka + Gyro * Kg + dGyro * Kdg + XSpeed + (dMove)
* Kdm + Move * Km + ddMove * Kddm;
Экспериментально подобранные коэффициенты находятся в файле defin.h:
double
Km = 0.0325;
double
Kdm = 0.03;
double
Kddm = 0.00025;
double
Ka = 1200.0 / ( PI );
double
Kg = 300.0 / ( PI);
double
Kdg = 0.0025;
Чтобы сопоставить расчет Speed с используемой при вызове прерывания скорости колеса за 100 секунд, скорость умножается на полученный экспериментально коэффициент:
Speed *= 400.0;
Далее рассчитанные и проверенные на вхождение в установленные лимиты скорости колес передаются на управление моторами:
SetSpeed(speed_L, speed_R);
XSpeed = (speed_L + speed_R) / 800.0 — расчет скорости (Sn-1 в формуле 8.5)
робота для следующей итерации расчета.
Листинг 8.3. Робот научился стоять на месте (файл listing_8_3.ino)
#include "defin.h"
#include "gyro_acsel.h"
#include "motorstep.h"
// Описание моторов
#include "irq_robot.h"
// Работа с прерываниями
void setup() {
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы.
motor_off();
// Инициализируем генерацию шагов моторов
timer_setup();
Serial.begin(115200);
giroscop_setup();
Глава 8
234
delay(100);
Calc_CompensatorZ(4000);
t0 = micros() - 5000;
t2 = micros();
Speed = 0;
flag_crash = true;
time_stop_move = millis()+3000;
}
uint32_t micros_;
void loop()
{
int32_t speed_L;
int32_t speed_R;
t_period = 5000;
static int i = 0; // Для торможения вывода
micros_ = micros();
if (micros_ < t2) return;// Если период < t2 анализ. прошло 5000
// если да, измеряем угловую скорость.
// Опрос гироприбора:
Data_mpu6050();
dt = micros() - t0; // Длительность предыдущего периода
регулирования.
t0 += dt; // Точка начала нового периода регулирования.
double Dt = double(dt) * 0.000001;
// Нужно поймать угол, рядом с которым угловая скорость
// колеблется в районе нуля.
// Расчет угла по показаниям акселерометра
Acsel = (atan2(AcX, AcZ)) - PI / 2.0; // * RAD_TO_DEG;
// Скорость угловая (в радианах) - падения
Gyro = - (double(GyY) - CompensatorY)
* _1_d_131;
// Комплементарный фильтр
AcYsum = ONE_ALFA * (AcYsum + Gyro * Dt) + ALFA * Acsel;
t2 = t0 + t_period;
if ( flag_crash)
{
Балансирующие роботы
if(time_stop_move> millis()) return;
// Если робот поднят после падения
if (abs(AcYsum * RAD_TO_DEG) < 10)
{
motor_on();
counter_stepR = 0;
counter_stepL = 0;
flag_crash = false;
}
else return;
}
// Если вошли в критичный режим - робот упал
if (abs(Move) > MoveLimit)
{
flag_crash = true;
XSpeed = 0;
Speed = 0;
speed_L = 0;
speed_R = 0;
counter_stepR = 0;
counter_stepL = 0;
speed_xxx_L = 0;
newSpeedflag_L
= true;
speed_xxx_R = 0;
newSpeedflag_R
= true;
dGyro = 0;
Move = 0;
dMove = 0;
ddMove = 0;
dMoveOld = 0;
motor_off();
// 6 секунд на стабилизацию после падения
time_stop_move = millis() + 6000;
GyroOld = 0;
return;
}
235
236
Глава 8
XL = counter_stepL; counter_stepL = 0;
XR = counter_stepR; counter_stepR = 0;
dMove = double(XL + XR) * 0.5 / Dt;
ddMove = ddMove * 0.9 + (dMove - dMoveOld) * 0.1 / Dt;
dMoveOld = dMove;
Move += (XL + XR) / 2.0; //Это как раз пройденный путь
dGyro = 0.9 * dGyro + 0.1 * ((Gyro - GyroOld) / Dt);
GyroOld = Gyro;
Speed = (AcYsum) * Ka + Gyro * Kg + dGyro * Kdg + XSpeed + (dMove)
* Kdm + Move * Km + ddMove * Kddm;
Speed *= 400.0;
speed_L = (Speed);
speed_R = (Speed);
speed_L = constrain(speed_L, -maxSPEED, maxSPEED);
speed_R = constrain(speed_R, -maxSPEED, maxSPEED);
// Speed - это число шагов за 100 секунд
SetSpeed(speed_L, speed_R);
// Возвращаем обработанное значение скорости в первоначальную формулу
XSpeed = (speed_L + speed_R) / 800.0;
}
Подключаем внешнее управление и подъемный рычаг
(окончательная версия программы)
Проведем небольшую модернизацию робота, установив на него сервомотор с
рычагом. Цель этого усовершенствования — дать роботу возможность самостоятельно вставать в случае падения. Если у вас под рукой сервомотора не оказалось — не страшно, эта программа будет работать, только робота придется вам
самому поднимать руками.
Сервомотор (рис. 8.15) используется с фланцем, имеющим отверстие с зубчатой внутренней поверхностью для установки на вал сервомотора, и отверстия
Балансирующие роботы
237
диаметром 2 мм по периметру для крепления рычагов винтами или шурупами.
Фланец после установки на сервомотор фиксируется винтом 3 мм. В места крепления сервомотора на роботе устанавливаются резиновые вибропрокладки.
Фланец сервомотора устанавливаем в подъемный рычаг и фиксируем шурупами либо винтами по периметру (рис. 8.16). Затем крепим подъемный рычаг с
фланцем к сервомотору, установленному на робота (рис. 8.17), — робот с этими
доработками будет выглядеть, как показано на рис. 8.18.
Теперь надо проверить диапазон движений. Если сервомотор имеет фиксатор,
предотвращающий прокручивание вала на 360 градусов, добиваемся, чтобы
рычаг опускался в обе стороны на одинаковую величину угла. Если вал сервомотора прокручивается полностью, операцию настройки поворота вала можно
Рис. 8.15. Сервомотор малой мощности
Рис. 8.16. Подъемный рычаг с установленным на него фланцем
Глава 8
238
Рис. 8.17. Установка рычага на сервомотор робота
Рис. 8.18. Робот с установленным подъемным рычагом
Балансирующие роботы
239
будет провести только после электрического подключения и проведения тестов:
поворотов на 0 и на 180 градусов.
В балансирующем состоянии рычаг поднят. После падения, когда роботу необходимо подняться, рычаг опускается в сторону, на которой лежит робот, робот
приподнимается, после чего включается алгоритм балансировки. Робот заканчивает подъем колесами и начинает балансировать.
Сервомотор, подобный показанному на рис. 8.15, не должен питаться напряжением более 6 вольт, а в режиме нагрузки может потреблять ток до одного ампера. Следовательно, его требуется подключать к понижающему стабилизатору.
Воспользуемся для этого линейным стабилизатором L7805CV. Стабилизатор
для сервомотора должен быть отдельным — использование общего стабилизатора для контроллера ESP32 и сервомотора приводит к нестабильной работе
ESP32. Логически сервомотор будет управляться от контакта GPIO 19. Схема
подключения питания к сервомотору и управления подъемным рычагом приведена на рис. 8.19.
Рис. 8.19. Схема подключения питания и управления подъемного рычага
Глава 8
240
Расчетная часть
Для того чтобы робот смог двигаться, нужно создать управляемое отклонение
от балансного состояния в нужную сторону, но из формулы 8.5 любое отклонение будет сразу компенсироваться звеньями: интегральным — по скорости
Move×Km и пропорциональным — по скорости dMove×Kdm.
Если внести отклонения именно при расчете этих звеньев на определенную
величину, то компенсация отклонения будет не полной, и робот покатится.
Соответственно, вместо величин:
Move+ = (Sn-1)×Dt, dMove = Sn-1
надо принять величины:
Move+ = (Sn-1–Sz)×Dt, dMove = Sn-1–Sz
где Sz — заданная скорость движения,
и формулу:
Sn = Sn-1 + Ang×Kp + Gy×Kg + Move×Km + dMove×Kdm,
(8.7)
Изменение расчета этих звеньев на величину Sz, которая задает скорость движения в шагах шаговых моторов, приведет к управляемому (с заданной скоростью) движению робота.
Для поворотов робота нужно скорость правого колеса увеличить на величину
скорости поворота, а левого — уменьшить на такую же величину или наоборот.
Важно, чтобы при этом не менялось общее изменение скорости робота, заданное формулой балансировки 8.5. Если сложить скорости обоих моторов и поделить на 2, должна получиться скорость, рассчитанная для балансировки.
Реализуем эти соображения в программе.
Программная часть робота с удаленным управлением и рычагом
Программа состоит из 7 файлов (рис. 8.20):
• listing_8_4.ino — инициализирующая часть и основная функция loop(),
в которой производится расчет углов наклона робота, рассчитывается ПИДрегулятор, подается команда на смену скоростей моторов;
• defin.h — описание переменных;
• gyro_acsel.h — обслуживание MPU-6050;
• irq_robot.h — обработка прерываний по таймеру, генерация шагов моторов;
Балансирующие роботы
241
Рис. 8.20. Блоки (файлы) программы в Arduino IDE
• motorstep.h — обслуживание шаговых моторов, инициализация, включение, отключение;
• move_case.h — обработка команд от смартфона на изменение движения робота;
• servo_hand.h — инициализация, функция подъема и опускания «серворуки».
Рассмотрим здесь только те листинги (файлы), которые претерпели серьезные
изменения.
• Листинг 8.4.1 (файл defin.h) содержит описание глобальных переменных и
некоторых констант. Основная цель его создания — выделение переменных
общего назначения в отдельный файл для повышения читаемости головного файла программы.
Листинг 8.4.1. Глобальные переменные и константы (файл defin.h)
float alfa = 0.001;
// Переменные управления интервалами опроса гироприбора и интервалами
управления
int32_t dt;
int32_t t0;
int32_t t2;
double GyXsum0;
// Длина окружности колеса 25 см, 250 мм
// Количество шагов на оборот - 200 (360/1.8)
// Количество микрошагов - 200*16 = 3200 на оборот
Глава 8
242
int32_t t_period;
double CommandSpeed = 0; // Скорость управляемого движения на 100 сек.
double OldCommandSpeed = 0; // Скорость управляемого движения из
// предыдущего опроса
double Turn = 0; // Поворот.
volatile int32_t counter_stepL; // Счетчик шагов левого мотора
volatile int32_t counter_stepR; // Счетчик шагов правого мотора
bool flag_crash = true;
bool flag_time_standup = true;// Флаг того, что робот встает.
uint32_t time_standup = 0; // Время на подъем робота
double XSpeed = 0; // Реальная скорость по шагам в моторах
double
OldXSpeed = 0; // Реальная скорость по шагам в моторах
// Нужно его использовать
int32_t XL, XR; // Счетчик тактов на колесо в текущей итерации
int32_t OldCommandSpeed_dMove = 0;
bool flag_to_off_robot
= false; // Флаг выключения робота
int32_t time_to_off_robot = 0; // Время на присадку робота
uint32_t time_stop_move=0;
double
Km = 0.0325;
double
Kdm = 0.03;
double
Kddm = 0.00025;
double
Ka = 1200.0 / ( PI );
double
Kg = 300.0 / ( PI);
double
Kdg = 0.0025;
// Предел интегрального звена Move,
// накопление больше которого считается, что робот упал
double MoveLimit = 0.5*Ka/Km;
double Speed = 0;
double OldSpeed = 0;
double OldGyro = 0;
double Move = 0; // Пройденный путь.
double dMove = 0;
double ddMove = 0;
double dMoveOld = 0;
double dGyro = 0;
double GyroOld = 0;
Балансирующие роботы
243
• Листинг 8.4.2 (файл move_case.h) содержит код обработки команд управления роботом от смартфона:
* переменные MOVE_PERIOD и time_start_move — отслеживание потери
связи со смартфоном для остановки робота;
* константы MOVSPEED0…MOVSPEED10 — для задания максимальной скорости робота;
* переменная ASPEED — хранит текущую максимальную скорость управляемого движения;
* функция BT_input() — анализирует наличие принятой от смартфона
команды, при наличии в операторе switch(bt_input) выбирается действие в зависимости от принятой команды;
* благодаря функции constrain(значение, нижний лимит, верхний
лимит), ограничивающей значения определенным диапазоном, скорости
изменяются не на максимальную возможную величину, а на приращение
DeltaSpeed_100sec. Следующее приращение — только через 50 миллисекунд (когда придет следующая команда).
Листинг 8.4.2. Обработка внешних команд (файл move_case.h)
uint32_t MOVE_PERIOD = 300;
uint32_t
time_start_move = 0;
const int32_t
MOVSPEED0 = (maxSPEED/100)*3;
const int32_t
MOVSPEED1 = (maxSPEED/100)*5;
const int32_t
MOVSPEED2 = (maxSPEED/100)*10;
const int32_t
MOVSPEED3 = (maxSPEED/100)*15;
const int32_t
MOVSPEED4 = (maxSPEED/100)*20;
const int32_t
MOVSPEED5 = (maxSPEED/100)*25;
const int32_t
MOVSPEED6 = (maxSPEED/100)*30;
const int32_t
MOVSPEED7 = (maxSPEED/100)*35;
const int32_t
MOVSPEED8 = (maxSPEED/100)*40;
const int32_t
MOVSPEED9 = (maxSPEED/100)*45;
const int32_t
MOVSPEED10 = (maxSPEED/100)*50;
int32_t ASPEED = MOVSPEED2;
int32_t DeltaSpeed_100sec = 30000;
bool BT_input()
{
static char bt_input;
if (SerialBT.available())
Глава 8
244
{
bt_input = (char)SerialBT.read();
}
else
{
if(time_start_move < millis())
{
// CommandSpeed = 0;
// Обнуляем скорость управляемого движения.
if(CommandSpeed>0) CommandSpeed = constrain(CommandSpeedDeltaSpeed_100sec,0,CommandSpeed);
// Обнуляем скорость управляемого движения.
if(CommandSpeed<0) CommandSpeed = constrain(CommandSpeed+DeltaSp
eed_100sec,CommandSpeed,0);
// Обнуляем скорость управляемого движения.
Turn = 0;
}
return false;
}
time_start_move = millis() + MOVE_PERIOD;
//=====================
switch (bt_input)
{
case 'S': // вперед
//
CommandSpeed = 0;
// Обнуляем скорость управляемого движения.
if(CommandSpeed>0) CommandSpeed = constrain(CommandSpeedDeltaSpeed_100sec,0,CommandSpeed);
// Обнуляем скорость управляемого движения.
if(CommandSpeed<0) CommandSpeed = constrain(CommandSpeed+D
eltaSpeed_100sec,CommandSpeed,0);
// Обнуляем скорость управляемого движения.
Turn = 0;
break;
case 'F': // вперед
Turn = 0;
CommandSpeed = constrain(CommandSpeed+DeltaSpeed_100sec,Co
mmandSpeed, ASPEED);
Балансирующие роботы
245
break;
case 'B': //назад
CommandSpeed = constrain(CommandSpeed-DeltaSpeed_100sec,ASPEED,CommandSpeed);
Turn = 0;
break;
case 'L':
// Влево на месте
Turn = -ASPEED / 2;
// Обнуляем скорость управляемого движения.
if(CommandSpeed>0) CommandSpeed = constrain(CommandSpeedDeltaSpeed_100sec,0,CommandSpeed);
if(CommandSpeed<0) CommandSpeed = constrain(CommandSpeed+D
eltaSpeed_100sec,CommandSpeed,0);
break;
case 'R':
// Вправо на месте
Turn = ASPEED / 2;
// Обнуляем скорость управляемого движения.
if(CommandSpeed>0) CommandSpeed = constrain(CommandSpeedDeltaSpeed_100sec,0,CommandSpeed);
if(CommandSpeed<0) CommandSpeed = constrain(CommandSpeed+D
eltaSpeed_100sec,CommandSpeed,0);
break;
case 'G':
// Влево вперед
//CommandSpeed = ASPEED; //
CommandSpeed = constrain(CommandSpeed+DeltaSpeed_100sec,Co
mmandSpeed, ASPEED);
Turn = -ASPEED / 2;
break;
case 'I':
// Вправо вперед
//CommandSpeed = ASPEED; //
CommandSpeed = constrain(CommandSpeed+DeltaSpeed_100sec,Co
mmandSpeed, ASPEED);
Turn = ASPEED / 2;
break;
case 'H':
// Вправо
break;
case 'J':
break;
// Влево
Глава 8
246
case '0':
ASPEED = MOVSPEED0;
break;
// Скорость 10%
case '1':
ASPEED = MOVSPEED1;
break;
// Скорость 20%
case '2':
ASPEED = MOVSPEED2;
break;
// Скорость 30%
case '3':
ASPEED = MOVSPEED3;
break;
// Скорость 40%
case '4':
ASPEED = MOVSPEED4;
break;
// Скорость 50%
case '5':
ASPEED = MOVSPEED5;
break;
// Скорость 60%
case '6':
ASPEED = MOVSPEED6;
break;
// Скорость 70%
case '7':
ASPEED = MOVSPEED7;
break;
case '8':
// Скорость 80%
ASPEED = MOVSPEED8;
break;
// Скорость 90%
case '9':
ASPEED = MOVSPEED9;
Балансирующие роботы
247
break;
// Скорость 100%
case 'q':
ASPEED = MOVSPEED10;
break;
case 'V':
break;
case 'v':
break;
case 'X':
break;
case 'x':
break;
default:
break;
}
}
• Листинг 8.4.3 (файл servo_hand.h):
∗ здесь вначале подключается библиотека ESP32Servo.h — ее предварительно требуется установить в систему из внутреннего репозитория
Arduino IDE (рис. 8.21);
Рис. 8.21. Установка библиотеки ESP32Servo из репозитория
Глава 8
248
∗ затем создается объект one_hand типа Servo, переменные minUs и maxUs
задают ширину импульсов для управления ШИМ сервопривода, one_
handGPIO — контакт GPIO управления;
∗ функция servo_hand_setup() инициализирует сервопривод;
∗ функция servo_hand_bottom(double test_angle) проверяет, в какую
сторону наклонен робот, и поворачивает рычаг в попытке его поднять;
∗ функция servo_hand_up() — если робот поднят, эта функция поднимает
серворычаг и через 4 секунды отключает сервопривод (экономия заряда).
Листинг 8.4.3. Управление рычагом подъема (файл servo_hand.h)
#include <ESP32Servo.h>
Servo one_hand;
// Published values for SG90 servos; adjust if needed
int minUs = 800;
int maxUs = 2500;
int one_handGPIO = 19;
bool one_handattach = false;
void servo_hand_setup()
{
one_hand.setPeriodHertz(50);
// Standard 50hz servo
//one_hand.attach(one_handGPIO, minUs, maxUs);
//one_hand.write(90);
one_handattach = false;
}
void servo_hand_bottom(double test_angle)
{
if (test_angle < - 20.0) //Если вошли в критичный режим
{
if (one_handattach == false)
{
one_handattach = true;
one_hand.attach(one_handGPIO, minUs, maxUs);
}
one_hand.write(200);
}
Балансирующие роботы
else if (test_angle >
249
20.0) //Если вошли в критичный режим
{
if (one_handattach == false)
{
one_handattach = true;
one_hand.attach(one_handGPIO, minUs, maxUs);
}
one_hand.write(0);
}
}
void servo_hand_up()
{
static uint32_t timess = 0;
if (one_hand.attached())
{
if (one_hand.read() != 100)
{
one_hand.write(100);
timess = millis() + 4000;
}
else
{
if (timess < millis())
{
one_hand.detach();
one_handattach = false;
}
}
}
}
• Листинг 8.4.4 (файл listing_8_4.ino):
∗ в первых строках подключается библиотека BluetoothSerial.h и объявляется объект SerialBT типа BluetoothSerial — тем самым создается возможность принимать и получать данные по Bluethooth, работая
как с Serial-интерфейсом;
Глава 8
250
∗ далее по директиве #include к головному модулю подключаются остальные файлы проекта;
∗ функция setup()— инициализирует сервомотор, шаговые моторы, запускает прерывание по таймеру (каждые 10 микросекунд), открывает
Serial-порт на скорости 115 200 бит в секунду, открывает SerialBTпорт и роботу в среде Bluetooth присваивает имя BHV_M, запускает расчет
(на 4 секунды) отклонений нуля гироскопа (нельзя трогать робота в это
время), устанавливает значения в переменные времени для отслеживания длительности периода перерасчета скорости;
∗ функция loop() автоматически повторяется в бесконечном цикле.
Вначале проверяется, подошло ли время для начала расчета (5000 микросекунд), если подошло, производится расчет угла наклона робота по
описанному ранее алгоритму (комплементарный фильтр). Далее робот
проверяется на падение (аналогично программе, в которой мы научили
робота стоять, — см. листинг 8.3) — различие в том, что, в случае падения, робот включает серворычаг и пытается встать (функция servo_
hand_bottom(AcYsum*RAD_TO_DEG)). Далее идет расчет скорости на следующий период по формуле 8.5 с учетом дополнений (см. формулу 8.7)
на движение:
Строки:
speed_L = (Speed + Turn);
speed_R = (Speed - Turn);
служат для организации поворота робота;
∗ вызов функции SetSpeed(speed_L, speed_R) приводит к установке
флагов наличия новых скоростей. Эти флаги проверяются в функции обработки прерывания по таймеру, и при необходимости робот начинает
изменять свою скорость.
Листинг 8.4.4. Головной модуль (файл listing_8_4.ino)
#include <BluetoothSerial.h>
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and
enable it
#endif
BluetoothSerial SerialBT;
#include "defin.h"
#include "gyro_acsel.h"
#include "motorstep.h"
// Описание моторов
Балансирующие роботы
251
#include “irq_robot.h”
// Работа с прерываниями
#include “move_case.h”
// Реагирование на команды
#include "servo_hand.h"
// Подъемный рычаг
void setup() {
// Инициализация рычага
servo_hand_setup();
// Инициализируем моторы
setup_motor_system();
// Выключаем моторы.
motor_off();
// Инициализируем генерацию шагов моторов
timer_setup();
Serial.begin(115200);
Serial.println();
SerialBT.begin("BHV_M"); // Имя робота в BT
giroscop_setup();
delay(100);
Calc_CompensatorZ(4000);
t0 = micros() - 5000;
t2 = micros();
Speed = 0;
flag_crash = true;
time_start_move = millis();
time_stop_move = millis();
}
uint32_t micros_;
void loop()
{
int32_t speed_L ;
int32_t speed_R;
t_period = 5000;
Глава 8
252
static int i = 0; // Для торможения вывода
micros_ = micros();
if (micros_ < t2) return;// Если период < t2 анализ. прошло 5000
// если да, измеряем угловую скорость.
BT_input();
// Опрос гироприбора:
Data_mpu6050();
dt = micros() - t0; // Длительность предыдущего периода
регулирования.
t0 += dt; // Точка начала нового периода регулирования.
double Dt = double(dt) * 0.000001;
// Нужно поймать угол, рядом с которым угловая скорость
// колеблется в районе нуля.
// Расчет угла по показаниям акселерометра
Acsel = (atan2(AcX, AcZ)) - PI / 2.0; // * RAD_TO_DEG;
// скорость угловая (в радианах) - падения
Gyro = - (double(GyY) - CompensatorY)
* _1_d_131;
// Комплементарный фильтр
AcYsum = ONE_ALFA * (AcYsum + Gyro * Dt) + ALFA * Acsel;
t2 = t0 + t_period;
if ( flag_crash)
{
if ((time_stop_move - 3000) < millis())
{
// Запускаем подъемный рычаг
servo_hand_bottom(AcYsum * RAD_TO_DEG);
}
if ((time_stop_move - 1000) < millis())
{
if (abs(AcYsum * RAD_TO_DEG) < 40) // Если поднят
{
motor_on();
}
Балансирующие роботы
253
if (time_stop_move
< millis())
{
if (abs(AcYsum * RAD_TO_DEG) < 40) // Если поднят
{
flag_crash = false;
counter_stepR = 0;
counter_stepL = 0;
}
else {
//Move = MoveLimit * 2;
}
}
}
counter_stepR = 0;
counter_stepL = 0;
return;
}
// Если вошли в критичный режим - , робот упал, но из-за ошибки
// гироскопа показал достаточное отклонение.
if (abs(Move) > MoveLimit) // Если вошли в критичный режим
{
flag_crash = true;
XSpeed = 0;
Speed = 0;
OldCommandSpeed = 0;
CommandSpeed = 0;
speed_L = 0;
speed_R = 0;
counter_stepR = 0;
counter_stepL = 0;
speed_xxx_L = 0;
newSpeedflag_L
= true;
speed_xxx_R = 0;
newSpeedflag_R
dGyro = 0;
Move = 0;
= true;
Глава 8
254
dMove = 0;
ddMove = 0;
dMoveOld = 0;
motor_off();
Turn = 0;
time_stop_move = millis() + 6000; // Секунда на стабилизацию
GyroOld = 0;
servo_hand_up();
return;
}
if ((time_stop_move + 2000) < millis()) servo_hand_up();
XL = counter_stepL; counter_stepL = 0;
XR = counter_stepR; counter_stepR = 0;
double dMoveX = double(XL + XR) * 0.5 / Dt;
OldCommandSpeed_dMove = OldCommandSpeed;
dMove = dMoveX - OldCommandSpeed_dMove;
ddMove = ddMove * 0.9 + (dMove - dMoveOld) * 0.1 / Dt;
dMoveOld = dMove;
Move += (XL + XR) / 2.0 + Dt * ( - OldCommandSpeed); // Это как раз
// пройденный путь
dGyro = 0.9 * dGyro + 0.1 * ((Gyro - GyroOld) / Dt);
GyroOld = Gyro;
Speed = (AcYsum) * Ka + Gyro * Kg + dGyro * Kdg + XSpeed +
(dMove) * Kdm + Move * Km + ddMove * Kddm;
OldCommandSpeed = CommandSpeed / 100.0;
Speed *= 400.0;
speed_L = (Speed + Turn);
speed_R = (Speed - Turn);
speed_L = constrain(speed_L, -maxSPEED, maxSPEED);
speed_R = constrain(speed_R, -maxSPEED, maxSPEED);
Балансирующие роботы
255
//Speed - это число шагов за 100 секунд
SetSpeed(speed_L, speed_R);
// Возвращаем обработанное значение скорости в первоначальную
формулу
XSpeed = (speed_L + speed_R) / 800.0;
}
Ф а й ло в ы й а рх ив
Полные версии листингов, приведенных в этой главе, вы найдете в соответствующих
папках сопровождающего книгу файлового архива (см. приложение).
В этой главе мы рассмотрели пример поддержания роботом равновесия. Программа, обеспечивающая это, непростая, но мы надеемся, что вы справились.
Надеемся, что вы смогли понять основы регулирования и разобраться, в чем
различия пропорционального, интегрального и дифференциального звеньев.
Подобные регуляторы применяются не только в балансировке роботов, но и для
поддержания скорости и направления движения различных устройств, для поддержания температуры.
Чтобы упростить понимание описанных здесь процедур, к выходу книги на
канале YouTube: https://www.youtube.com/user/momotmvu будет выпущен
учебный ролик. В нем мы дополнительно разъясним вопросы настройки балансирующего робота, описанного в этой книге.
*
*
*
Эта глава завершает книгу, однако у нас есть еще много проектов, про которые
хочется рассказать. Будет интересно узнать ваше мнение о том, какие проекты
стоит рассмотреть в будущем, и какие контроллеры предпочитают использовать
в своих проектах наши читатели.
ПРИЛОЖЕНИЕ. СОДЕРЖАНИЕ
ФАЙЛОВОГО АРХИВА
Папки
Описание
Главы
listing_2_1
Файл listing_2_1.ino соответствует листингу 2.1 — программа
веб-сервера, демонстрирующего температуру внутри контроллера ESP32. Веб-сервер основан на стандартном примере для ESP32 (AdvancedWebServer), но вместо рандомного
графика мы строим график температуры ядра ESP32
2
listing_3_1
Файл listing_3_1.ino соответствует листингу 3.1 — программа запуска шагового мотора. Запускаем шаговый мотор,
делаем 600 шагов, время между шагами 3 миллисекунды.
Когда все шаги сделаны, двигатель останавливается, а через
500 миллисекунд с него снимается напряжение
3
Файл listing_3_2.ino соответствует листингу 3.2 — программа вращения шагового мотора с заданным ускорением
и иными параметрами:
listing_3_2
•
MAXi = 6000; // Количество шагов;
•
min_steplong = 200; // Длительность самого корот-
кого шага в микросекундах (максимальная скорость);
•
3
max_steplong = 3000; // Длительность стартового
шага, первый шаг после остановки имеет данную длину;
•
acceleration = 10000; // Ускорение в шагах на секунду в квадрате. С данным ускорением раскручивается
вал двигателя при старте и с данным отрицательным
ускорением тормозит
Файл listing_3_3.ino соответствует листингу 3.3 — программа вращения колес по вводимым данным (управление мотором через последовательный порт).
listing_3_3
В отличие от предыдущего примера, этот пример позволяет в окне Монитора порта задать параметры: количество
шагов, максимальную скорость, ускорение. Скорость порта
устанавливается 115 200 бит/с.
Параметры задаются путем передачи последовательности
из трех параметров:
•
S+число — задается скорость;
•
М+число — задается количество шагов (знак минус
изменяет направление);
3
Приложение
258
Папки
Описание
•
•
Главы
А+число — задает ускорение;
символ E — завершает ввод команды и запускает цикл
исполнения введенной команды.
Пример: S3000M-6000A4000E. Здесь задаются максимальная скорость 3000 шагов/сек, количество шагов 6000 в инверсном направлении, ускорение 4000 шагов/сек2
Файл listing_4_1.ino соответствует листингу 4.1 — тест вращения колес.
listing_4_1
listing_4_2
listing_5_1
Проводится тестирование правильности сборки робота
путем генерации шагов с малой скоростью на оба колеса
робота. При этом задействуются 5 контактов GPIO (15, 4,
13, 16, 2):
• STEP_L 15 — генерация шагов левого колеса;
• STEP_R 4 — генерация шагов правого колеса;
• DIR_L 13 — направление вращения левого колеса;
• DIR_R 16 — направление вращения правого колеса;
• STEPER_EN 2 — включение обоих моторов.
Базовая программа управления роботом командами, поступающими по Bluetooth. Программа состоит из четырех
файлов:
• listing_4_2.ino — головной файл программы;
• motorstep.h — инициализация шаговых моторов;
• irq_robot.h — обработка прерываний, генерация шагов
по таймеру;
• move_case.h — обработка команд, поступающих по
Bluetooth.
Файл listing_5_1.ino соответствует листингу 5.1 — пример
работы с внутренней энергонезависимой памятью.
Пример позволяет понять принципы работы с Flashпамятью, распаянной на плате контроллера ESP32
4
4
5
Файл listing_5_1b.ino соответствует листингу 5.1b — программа проверки работы кнопок.
listing_5_1b
listing_5_2
Этот простой пример считывания величины напряжения
с контакта GPIO 32 демонстрирует изменение величины
считываемого напряжения в зависимости от нажатия определенной кнопки
Программа самостоятельного повторения роботом пройденного пути. Программа состоит из четырех файлов:
• listing_5_2.ino — головной файл программы (в том числе работа с энергонезависимой памятью и обработка
нажатий кнопок);
5
5
Содержание файлового архива
Папки
Описание
•
•
•
259
Главы
motorstep.h — инициализация шаговых моторов;
irq_robot.h — обработка прерываний, генерация шагов
по таймеру, запись в массив шагов, сделанных роботом;
move_case.h — обработка команд, поступающих по
Bluetooth.
Программа обхода препятствий и прохождения лабиринта.
Программа состоит из четырех файлов:
•
listing_6_1
•
•
•
listing_6_1.ino — головной файл программы (анализ
наличия препятствий и генерация команд управления
скоростью моторов);
motorstep.h — инициализация шаговых моторов;
irq_robot.h — обработка прерываний, генерация шагов
по таймеру;
parallelsonar.h — работа с ультразвуковыми датчиками
измерения расстояния от датчиков до препятствий.
6
Программа управления роботом телеприсутствия (в качестве камеры на роботе — смартфон) — является адаптированной копией программы из листинга 4.1 с измененной
системой обработки внешних команд. Программа состоит
из четырех файлов:
listing_7_1
•
•
•
•
listing_7_1.ino — головной файл программы;
motorstep.h — инициализация шаговых моторов;
irq_robot.h — обработка прерываний, генерация шагов
по таймеру;
move_case.h — обработка команд, поступающих по
Bluetooth.
7
Программа управления роботом телеприсутствия (в качестве камеры на роботе — камера ESP32-CAM). Робот принимает команды от UART-порта и формирует поступательные движения и повороты. Программа состоит из четырех
файлов:
listing_7_2
•
•
•
•
listing_7_2.ino — головной файл программы;
motorstep.h — инициализация шаговых моторов;
irq_robot.h — обработка прерываний, генерация шагов
по таймеру;
move_case.h — обработка команд, поступающих
от UART-порта.
Программа для загрузки в модуль ESP32-CAM (модернизированная программа управления камерой через сайт).
CameraWebServerEx Программа состоит из четырех файлов:
• CameraWebServerEx.ino — головной файл программы
(запуск камеры и веб-сервера);
7
7
Приложение
260
Папки
Описание
•
app_httpd.cpp — обработка видеосигнала, трансляция
видео, поддержка страницы сайта камеры;
•
camera_index.h — содержит массив со сжатым HTMLкодом страницы сайта камеры;
•
camera_pins.h — содержит описание контактов GPIO
для подключения различных модулей камеры к контроллерам ESP32.
Главы
Программа для загрузки в модуль ESP32-CAM (модернизированная программа управления роботом через сайт).
Программа состоит из четырех файлов:
CameraRobot
•
CameraRobot.ino — головной файл программы (запуск
камеры и веб-сервера);
•
app_httpd.cpp — обработка видеосигнала, трансляция
видео, поддержка страницы сайта камеры, передача
команд управления роботом по UART;
•
camera_index.h — содержит массив со сжатым HTMLкодом страницы сайта камеры;
•
camera_pins.h — содержит описание контактов GPIO
для подключения различных модулей камеры к контроллерам ESP32.
Дополнительно в папке в качестве примеров сохранены
файлы, сформированные из массивов файла camera_
index.h:
•
index_ov2640.html — распакованная страница сайта для
модуля камеры ov2640;
•
index_ov2640.html.gz — сжатая страница сайта для модуля камеры ov2640;
•
index_ov2640shot.html — распакованная измененная
страница сайта для модуля камеры ov2640;
•
index_ov2640shot.html.gz — сжатая измененная страница сайта для модуля камеры ov2640;
•
index_ov3660.html — распакованная страница сайта для
модуля камеры ov3660.
7
from_neco_apk
Содержит файл video_robot_v_1.1.6.apk — установочный
файл программы VideoRobot для смартфона на Android.
Программа VideoRobot устанавливается на смартфон, который будет использоваться в качестве камеры робота
7
listing_8_1
Программа опрашивает модуль MPU-6050, измеряет
и передает на последовательный порт компьютера рассчитанный с учетом комплементарного фильтра угол наклона
робота относительно оси Y
8
Содержание файлового архива
Папки
Описание
261
Главы
При использовании плоттера по последовательному порту
(встроен в Arduino IDE) можно получить три графика: кривую изменения угла на основании только угловой скорости,
на основании данных акселерометра, расчет комплементарного фильтра по данным акселерометра и гироскопа.
Программа состоит из двух файлов:
•
listing_8_1.ino — головной файл программы (расчеты
и передача данных);
•
gyro_acsel.h — работа с MPU-5060, инициализация
и опрос: получение данных об угловой скорости
и величины ускорения.
Программа подбора коэффициентов ПД-регулятора балансирующего робота. Программа состоит из пяти файлов:
listing_8_2
•
listing_8_2.ino — головной файл программы (расчеты,
управление скоростями колес);
•
defin.h — описание переменных, задействованных
в расчете балансировки робота;
•
gyro_acsel.h — работа с MPU-5060, инициализация
и опрос: получение данных об угловой скорости и величине ускорения;
•
motorstep.h — инициализация шаговых моторов;
•
irq_robot.h — обработка прерываний, генерация шагов
по таймеру.
8
Программа подбора коэффициентов ПИД-регулятора
балансирующего робота (робот учится стоять на месте).
Программа состоит из пяти файлов:
•
listing_8_3.ino — головной файл программы (расчеты,
управление скоростями колес);
•
defin.h — описание переменных, задействованных
в расчете балансировки робота;
•
gyro_acsel.h — работа с MPU-5060, инициализация
и опрос: получение данных об угловой скорости
и величине ускорения;
•
motorstep.h — инициализация шаговых моторов;
•
irq_robot.h — обработка прерываний, генерация шагов
по таймеру.
listing_8_3
8
Приложение
262
Папки
Описание
Главы
Программа управляемого движения балансирующего робота — дает возможность роботу не только самостоятельно
балансировать, но и двигаться с заданной скоростью и совершать повороты. Более того, робот получил сервомотор с
рычагом для самостоятельного подъема. Программа состоит из семи файлов:
listing_8_4
•
listing_8_4.ino — головной файл программы (расчеты,
управление скоростями колес);
•
defin.h — описание переменных, задействованных
в расчете балансировки робота;
•
gyro_acsel.h — работа с MPU-5060, инициализация
и опрос: получение данных об угловой скорости
и величине ускорения;
•
motorstep.h — инициализация шаговых моторов;
•
irq_robot.h — обработка прерываний, генерация шагов
по таймеру;
•
move_case.h — обработка команд, поступающих по
Bluetooth;
•
servo_hand.h — управление сервомотором подъемного
рычага.
8
Детали корпуса робота в формате STL для печати
на 3D-принтере и их количество для сборки робота:
model robot
•
борт.stl — 2 шт.;
•
колесо.stl — 2 шт.;
•
крепление аккумуляторов.stl — 1 шт.;
•
крепеж поворотного колеса.stl — 1 шт.;
•
палуба.stl — 1 шт.;
•
проставка.stl — 1 шт.;
•
рычажок.stl — 1 шт.;
•
связка бортов.stl — 3 шт.;
•
стойка палубы зад.stl — 2 шт.;
•
стойка палубы перед.stl — 2 шт.;
•
стойка палубы центр.stl — 1 шт.;
•
фиксатор датчиков.stl — 1 шт.
4–8
ПРЕДМЕТНЫЙ УКАЗАТЕЛЬ
А
Адресный светодиод 128
Акселерометр 206, 207, 209
Аналого-цифровой преобразователь,
АЦП 16
Аппаратный UART 15
Н
Несколько кнопок на одном аналоговом GPIO 125
О
Оперативная память 15
Б
Бесколлекторные моторы 42
В
Веб-сервер 35
Г
Гироскоп 208
П
ПИД-регулятор 220, 224, 240
Плоттер по последовательному соединению 217
Получение данных от MPU-6050 211,
217
Проброс портов 182
Пропорциональное регулирование
220–222
Д
Двоичный редактор 191
Дифференциальное регулирование
221, 223
Драйвер шагового мотора 43
И
Интегральное регулирование 221, 224
К
Коллекторные моторы 41
Конвертер USB-UART 184
Крутящий момент 44
Р
Регистр GPIO_OUT_REG 94
Роботы телеприсутствия 173
Ротор 43
С
Сервомоторы 42
Т
Таймеры 15
Тестовая программа движений робота
262
М
Механизм прерываний 84
Микросхема MPU-6050 206, 207, 211,
216, 217
Монитор порта 35, 40
У
Ультразвуковой дальномер 154
Ультразвуковой сигнал 156
Предметный указатель
264
Ц
Цифроаналоговое преобразование,
ЦАП 16
Э
Электронный архив 8
Электронный гироскоп 208, 209
Энергонезависимая память 15, 119
Ш
Шаговые моторы 42
ШИМ 221
Широтно-импульсная модуляция,
ШИМ 16
A
Arduino IDE 29–32, 35, 40
D
DRV8825 45
E
ESP32-CAM 184
I
I2C-интерфейс 16
Момот М.
www.bhv.ru
Мобильные роботы на базе Arduino,
2-е изд.
Отдел оптовых поставок:
E-mail: opt@bhv.ru
Мобильные роботы любой сложности легко и быстро!
«Эту книгу я составлял как руководство
для начинающих Конструкторов, людей,
которым нравится конструировать. А за
основу взял конструирование мобильных роботов на популярной платформе
Arduino, позволяющей реализовывать
как простейших, так и достаточно интеллектуальных роботов. Платформа
открытая, изготавливать дополнительные модули для нее может любой человек или организация, то же относится
и к программам.
Представленные проекты имеют единую колесную базу, но различаются
системами датчиков и программным
кодом. В процессе сборки вы научитесь программировать на платформе
Arduino, обращаться с электронными
компонентами, усвоите принципы действия датчиков, при помощи которых
роботы следят за внешним миром, научитесь удаленному управлению и сможете конструировать своих оригинальных роботов».
Михаил Момот, автор книги
Момот Михаил Викторович, доцент кафедры информационных систем Томского политехнического университета. Увлекается робототехникой, поклонник и пропагандист проекта
Arduino с 2014 года. Основатель неформального клуба робототехников «Лига роботов
ЮТИ ТПУ», объединяющего школьников, студентов, преподавателей и энтузиастов.
Григорьев А., Винницкий Ю.
www.bhv.ru
Игровая робототехника для юных
программистов и конструкторов:
mBot и mBlock
Отдел оптовых поставок:
E-mail: opt@bhv.ru
Эта книга о роботах и о том, как учиться новому, играя с ними. Познакомьтесь с симпатичным роботом mBot и
пройдите вместе с нами увлекательный
путь разработчика креативных игровых проектов.
Конструируя и программируя своего робота для игр, вы освоите вполне серьезные инструменты, научитесь использовать среду программирования mBlock,
различные электронные и механические
дополнения компании Makeblock, элементную базу Arduino.
Игровые проекты будут вначале очень
простыми и доступными любому начинающему, а затем все более сложными
и захватывающими.
Читайте, пробуйте, творите, создавайте новое. Дерзайте — вы талантливы!
А мы — немножко поможем!
Авторы работают в школе № 169 с углубленным изучением английского языка (СанктПетербург), и все проекты, с которыми вы познакомитесь в книге, опробованы учениками
этой школы на кружковых и внеурочных занятиях. Предыдущая книга авторов «Scratch и Arduino для юных программистов и конструкторов» быстро завоевала признательность юных
читателей и стала бестселлером.
Григорьев Александр Тихонович, психолог, преподаватель робототехники и конструирования, призер международных соревнований по робототехнике, автор книг и статей по
робототехнике, конструированию и использованию новых технологий в образовании.
Винницкий Юрий Анатольевич, кандидат педагогических наук, преподаватель информатики, неоднократный победитель профессиональных конкурсов, автор книг и статей по
робототехнике и конструированию, автор-разработчик более 200 электронных ресурсов
Единой коллекции цифровых образовательных ресурсов school-collection.edu.ru.
Тиммонс-Браун М.
www.bhv.ru
Робототехника на Raspberry Pi
для юных конструкторов и программистов
Отдел оптовых поставок:
E-mail: opt@bhv.ru
Опыт не требуется!
С помощью этой книги вы быстро и легко пройдете путь от неопытного пользователя до настоящего конструктора роботов. Вы начнете со сборки макета двухколесного робота на базе мини-компьютера Raspberry Pi и запрограммируете его на
популярном языке Python. Затем будете постепенно улучшать собранного робота,
добавляя новые функциональные возможности: он научится следовать линиям,
избегать препятствия, распознавать объекты и цвет с помощью компьютерного
зрения.
Вы узнаете, как:
• управлять роботом удаленно
с помощью пульта игровой
консоли Wii компании Nintendo;
• применять датчики для обхода
роботом препятствий;
• запрограммировать робота
следовать по линии;
• настроить робота, чтобы он сиял
огнями и воспроизводил звуки;
• наблюдать окружающий мир
«глазами робота» с помощью
видеокамеры Pi.
Работая с книгой, вы изучите основные
навыки работы с электронными компонентами, научитесь вычислять количество энергии, которое потребуется вашему роботу, познакомитесь с основами
программирования на языке Python и многое узнаете о работе с резисторами, светодиодами, двигателями и датчиками, что позволит вам создавать уже более сложные
конструкции.
Мэтт Тиммонс-Браун (Matt Timmons-Brown) — создатель самого популярного в мире
YouTube-канала, посвященного Raspberry Pi — The Raspberry Pi Guy. На этом канале он
выкладывает различные образовательные видеоматериалы для сообщества Raspberry Pi.
Канал Мэтта, запущенный в 2012 году, в настоящее время набрал более 5 миллионов
просмотров и поддерживается напрямую британским разработчиком процессоров ARM и
Фондом Raspberry Pi.
Мобильные роботы
на базе Аrduino + КНИГА
www.bhv.ru
Отдел оптовых поставок:
E-mail: opt@bhv.ru
Если вы хотите научиться конструировать мобильного робота, но у вас пока
нет знаний, опыта, деталей и электронных компонентов, то этот набор разработан специально для вас. В его состав
входят: плата Arduino, необходимые
электронные компоненты, двигатели,
4-колесное шасси, а также популярная
книга М. Момота «Мобильные роботы
на базе Arduino».
Вы сможете:
• сконструировать базовую модель
мобильного робота;
• дистанционно управлять роботом
с помощью IR-пульта или смартфона Android;
www.bhv.ru/books/robot
ISBN 978-5-9775-3774-2
• изучить базовые алгоритмы
управления роботом;
• модернизировать и обучить
базового робота
движению по линии,
выходу из лабиринта,
игре в кегельринг.
Умный дом на базе Аrduino.
Большой набор + КНИГА
www.bhv.ru
Отдел оптовых поставок:
E-mail: opt@bhv.ru
ISBN 978-5-9775-3988-3
www.bhv.ru/books/kits
Набор поможет вам с головой окунуться в удивительный мир Arduino. Вы научитесь подключать к плате Arduino и
программировать различные датчики
и модули, соберете макет «Умного домика» и разместите в нем собственную
«систему умного дома», которую можно впоследствии частично реализовать
и у себя в квартире или на даче.
Набор будет интересен как начинающим разработчикам, у которых еще
нет опыта создания проектов Arduino,
так и тем, кто уже сделал первые шаги
в разработке электронных проектов.
Интерфейс для iOS и Android
В состав набора входят:
• плата Arduino Uno;
• датчики;
• модули;
• светодиоды и дисплеи;
• серводвигатель и микронасос;
• блок питания 9 В и 4 батерейки AA;
• популярная книга Дж. Блума
«Изучаем Arduino: инструменты и
методы технического волшебства»;
• макет фанерного домика.
Система «умного полива»
Автономное питание
АRDUINO для изобретателей.
www.bhv.ru
Набор электронных компонентов
+ КНИГА
Отдел оптовых поставок:
E-mail: opt@bhv.ru
http://www.bhv.ru/books/200380
ISBN 978-5-9775-3988-3
В состав набора входят:
• плата, совместимая с Arduino
Uno;
• датчики;
• двигатели;
• светодиоды;
• базовые компоненты;
• макетная плата и провода;
• книга Б. Хуанга и Д. Ранберга
«Аrduino для изобретателей:
обучение электронике
на 10 занимательных
проектах».
Набор подготовлен по материалам популярной книги Б. Хуанга, Д. Ранберга
«Аrduino для изобретателей. Обучение электронике на 10 занимательных проектах», которая на практических примерах вводит читателя в увлекательный мир
разработчика электронных устройств на базе микроконтроллера Arduino. Выполнив описанные в книге проекты, вы научитесь не только собирать и программировать конкретные устройства, но и модифицировать их по своему желанию.
Кроме того, вы научитесь использовать подручные средства (картон, мячи для
пинг-понга и др.) для создания полноценных изделий. Сложность и трудность проектов повышается с каждым последующим.
•
•
•
•
•
•
•
Классический первый проект Arduino — мигание светодиодом
Миниатюрный светофор
Экран из светодиодов для отображения анимированных узоров и фигур
Увлекательная игра для тестирования быстроты реакции
Светочувствительный разноцветный ночник
Трудная, но занимательная игра балансирования мячика на балке
Миниатюрная теплица с автоматически управляемым вентилятором и окном
для регулировки температуры
• Робот-рисовальщик
• Устройство для хронометрирования гонок игрушечных
автомобилей
• Цифровая музыкальная труба
ИНТЕРНЕТ ВЕЩЕЙ.
www.bhv.ru
Набор для экспериментов
с контроллером ESP8266 + КНИГА
Отдел оптовых поставок:
E-mail: opt@bhv.ru
www.bhv.ru/books/198905
ISBN 978-5-9775-3932-6
Набор предназначен для тех, кто хочет разрабатывать мощные и недорогие проекты Интернета вещей (IoT)
на основе ESP8266. Он идеально подходит новичкам в области IoT и тем,
у кого уже есть опыт работы с платформой Arduino.
В набор входят:
• книга М. Шварц «Интернет вещей
с ESP8266»;
• две платы NodeMCU ESP8266;
• беспаечная макетная плата
и провода;
• датчики и электронные
компоненты;
• дверная защелка
соленоидного типа.
ESP8266 — это недорогой микроконтроллер с функцией Wi-Fi, при помощи которого различные объекты реализуют обмен данными. Благодаря недорогим модулям на основе
этого сетевого микроконтроллера, IoT переживает взрывной рост.
Вы научитесь:
• создавать и программировать
проекты IoT на основе ESP8266;
• считывать, отправлять и отслеживать данные через облачные
сервисы;
• применять ESP8266 для взаимодействия с социальными сетями
Twitter и Facebook;
• организовывать межмашинное
взаимодействие без участия
человека;
• построить простую систему
домашней автоматики
с управлением через облако;
• отправлять пользователям
ESP8266 сообщения по email,
SMS;
• создавать различные устройства
на основе ESP8266 (дверной замок
с управлением через облако,
индикатор курса Биткоина и др.).